From fe7992169ca8ad7538e6cb8c8ed5bc831b73677c Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Mon, 29 Jan 2024 14:07:35 +0100 Subject: [PATCH 01/32] ROS 2 lights animations (#221) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * add animation and image_animation class * controller node and pluginlib * add tests and fix issues * add animation images * add alpha channel * add charging animation with tests * update dummy controller * fix missing includes * add missing dep * Update panther_lights/include/panther_lights/animation/animation.hpp Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * Update panther_lights/include/panther_lights/animation/animation.hpp Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> * review changes * update tests --------- Co-authored-by: Paweł Kowalski <82044322+pkowalsk1@users.noreply.github.com> --- panther_lights/CMakeLists.txt | 67 +++++- .../animations/battery_critical.png | Bin 0 -> 2572 bytes panther_lights/animations/battery_low.png | Bin 0 -> 2793 bytes panther_lights/animations/strip01_purple.png | Bin 0 -> 964 bytes panther_lights/animations/strip01_red.png | Bin 0 -> 900 bytes panther_lights/animations/triangle01_blue.png | Bin 0 -> 1564 bytes panther_lights/animations/triangle01_cyan.png | Bin 0 -> 1679 bytes .../animations/triangle01_green.png | Bin 0 -> 1561 bytes .../animations/triangle01_orange.png | Bin 0 -> 1717 bytes .../animations/triangle01_purple.png | Bin 0 -> 1752 bytes panther_lights/animations/triangle01_red.png | Bin 0 -> 1560 bytes .../animations/triangle01_yellow.png | Bin 0 -> 1676 bytes .../panther_lights/animation/animation.hpp | 168 +++++++++++++ .../animation/charging_animation.hpp | 62 +++++ .../animation/image_animation.hpp | 82 +++++++ .../panther_lights/controller_node.hpp | 43 ++++ panther_lights/package.xml | 2 + panther_lights/plugins.xml | 8 + .../src/animation/charging_animation.cpp | 160 +++++++++++++ .../src/animation/image_animation.cpp | 164 +++++++++++++ panther_lights/src/controller_node.cpp | 87 +++++++ panther_lights/src/controller_node_main.cpp | 37 +++ panther_lights/test/test_animation.cpp | 194 +++++++++++++++ .../test/test_charging_animation.cpp | 201 ++++++++++++++++ panther_lights/test/test_image_animation.cpp | 223 ++++++++++++++++++ 25 files changed, 1488 insertions(+), 10 deletions(-) create mode 100644 panther_lights/animations/battery_critical.png create mode 100644 panther_lights/animations/battery_low.png create mode 100644 panther_lights/animations/strip01_purple.png create mode 100644 panther_lights/animations/strip01_red.png create mode 100644 panther_lights/animations/triangle01_blue.png create mode 100644 panther_lights/animations/triangle01_cyan.png create mode 100644 panther_lights/animations/triangle01_green.png create mode 100644 panther_lights/animations/triangle01_orange.png create mode 100644 panther_lights/animations/triangle01_purple.png create mode 100644 panther_lights/animations/triangle01_red.png create mode 100644 panther_lights/animations/triangle01_yellow.png create mode 100644 panther_lights/include/panther_lights/animation/animation.hpp create mode 100644 panther_lights/include/panther_lights/animation/charging_animation.hpp create mode 100644 panther_lights/include/panther_lights/animation/image_animation.hpp create mode 100644 panther_lights/include/panther_lights/controller_node.hpp create mode 100644 panther_lights/plugins.xml create mode 100644 panther_lights/src/animation/charging_animation.cpp create mode 100644 panther_lights/src/animation/image_animation.cpp create mode 100644 panther_lights/src/controller_node.cpp create mode 100644 panther_lights/src/controller_node_main.cpp create mode 100644 panther_lights/test/test_animation.cpp create mode 100644 panther_lights/test/test_charging_animation.cpp create mode 100644 panther_lights/test/test_image_animation.cpp diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 73d6141a2..ae32ca5ab 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -9,35 +9,82 @@ find_package(ament_cmake REQUIRED) find_package(image_transport REQUIRED) find_package(panther_gpiod REQUIRED) find_package(panther_msgs REQUIRED) +find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(yaml-cpp REQUIRED) include_directories(include) +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) + +add_library( + panther_animation_plugins SHARED src/animation/image_animation.cpp + src/animation/charging_animation.cpp) +ament_target_dependencies(panther_animation_plugins pluginlib) +target_link_libraries(panther_animation_plugins png yaml-cpp) + add_executable(driver_node src/driver_node_main.cpp src/driver_node.cpp src/apa102.cpp) ament_target_dependencies(driver_node image_transport panther_gpiod panther_msgs rclcpp sensor_msgs) +add_executable(controller_node src/controller_node_main.cpp + src/controller_node.cpp) + +ament_target_dependencies(controller_node rclcpp pluginlib sensor_msgs) +target_link_libraries(controller_node yaml-cpp) + add_executable(dummy_scheduler_node src/dummy_scheduler_node_main.cpp src/dummy_scheduler_node.cpp) ament_target_dependencies(dummy_scheduler_node image_transport rclcpp sensor_msgs) -install(TARGETS driver_node dummy_scheduler_node +install(TARGETS driver_node controller_node dummy_scheduler_node DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib) + +install(DIRECTORY animations launch DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY include/ DESTINATION include) +ament_export_include_directories(include) -# if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line -# skips the linter which checks for copyrights # comment the line when a -# copyright and license is added to all source files -# set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only -# works in a git repo) # comment the line when this package is in a git repo and -# when # a copyright and license is added to all source files -# set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() -# endif() +ament_export_libraries(panther_animation_plugins) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + + ament_add_gtest(${PROJECT_NAME}_test_animation test/test_animation.cpp) + target_include_directories( + ${PROJECT_NAME}_test_animation + PUBLIC $ + $) + target_link_libraries(${PROJECT_NAME}_test_animation yaml-cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_image_animation test/test_image_animation.cpp + src/animation/image_animation.cpp) + target_include_directories( + ${PROJECT_NAME}_test_image_animation + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_image_animation + ament_index_cpp pluginlib) + target_link_libraries(${PROJECT_NAME}_test_image_animation png yaml-cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_charging_animation test/test_charging_animation.cpp + src/animation/charging_animation.cpp) + target_include_directories( + ${PROJECT_NAME}_test_charging_animation + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_charging_animation + ament_index_cpp pluginlib) + target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp) +endif() ament_package() diff --git a/panther_lights/animations/battery_critical.png b/panther_lights/animations/battery_critical.png new file mode 100644 index 0000000000000000000000000000000000000000..d431f77e6e763e75cbcae339c1c6a43c6ab48396 GIT binary patch literal 2572 zcmZved03L^8pc0xL9k6wvqsG+MI{$XP29(A5^zZo%iLmI$jk+Ca?N219hYnoQBp?p zqe7EZL4lDCzp#Vvtq9z@94FwbO2h z!)ma19hh*-%j+dBSk2RaIqS(kvPWo+9NF7{v)Hky$uar#M|WBUJD$D9&5|}~BR$fi zXzOM4=7-RcGu1MwE3M*?)ylQe{Mxx|v|PJziEJ+MDqLk5f1SD&v$M*p9bw?4f8exc*L&Ev)3070mJGrj zEi2UjiZN8i>T1nFuNJ<}+u;)UPJO{c?O@JCOcK z+GJs>E~SGVw0%J!p242u64^pBe!UHdNM^~j0KeRn(4V|U*5 zw7aAw1=M)*(6$#pWS-=bR*XZ~8Qsc#_*Xeu%}T>7otc_)WB6-5hAoh%Eq4Jt0G#?Y zbu0{9llR4q;R4J<4As_jgh4>Q#)3HmwQSS@2s9TqGUnsmQ<2J#ze$x;pbuDciFN<@ z7$qoIo&!>**Ha{!N&0RhkmHc2n%97lL-(DM158G0*ZuYrRrO;*vLvgpp|>D;RfBgU)yFKi1W$um?cq)$B!O%&-cwAY1X zaO&baK^0b@`{EgoT31S*_gt!xZn~ z#6_mXo{P_WrB2dsx8yt?BgXee=Y@%8#fvST$tW{d?#PdY*wpwBp4@|#LT}Nlp>w3O zxZ8f>bq@CS-h%A!H%L)mBgHZzM<%Q*#pS|z7E_ZF8HwX7tUIlem?>k4EP^8ux|Q;p zu%(RLt<)fjzPYQuawTpfRi9dhosVmG5VdI<=aaK<&m>(GU)(LlD!TT;@*OPMPrjsZ zJ-H4dLFr7k2j!9dR2Q3&N}EBDdc2yUwGqwOBAySYT92@2p_y zUMb6$gAuwFff!c-&%wYynFQ)nS^%oPc@36~8pZ^oDJ>9Cv9M%@T`4Poa}55E8fVy* zu!1>n;Z&5W^H)%THQ_eNpoRZ9b3oxQD;xLXHi{lUiyv3ddYfWJq*Ws9He8JgyCNiJ z#)cR3LOInvZPli(O(EBHsqNT^P>Z)h;%>cUuy3r}D_A2sr1?+@@6QoJ+iu+zrt;RB zzQRf;p|o;RJu9)a!+Ll-=Rz1m|NK-|zCQ_o6ODXS?r@vJ8&L6}?2isH<`Q>_C}P^zIIHjC`lCW)&30a-*^ zj?NX!g(r$-6Db;=YNjf}CJEeVW+Fns$GK^ZxlUcj(c8<|yM_-_4u81uR?{`pF{~X& zWNs^*wrQOa5>Ldki@>u!>1pdW6EuY2^{BEuA5bYqrw$=yJne! zRa5DVkT32+u?uRMvb>EQdddXTLEq(6ThSa;@FSOQsd3t>3iWhObo@@k;}Uv=>3Wx; z$NAW3rp&kcI&R1FGCqOZ18jl_*sp&}w<2?{W`+ktMr36UdepTjLJPM4Xzx%7H0xnX z+oSNpZ!0pcz1urX1$c%3M!H`OtxP&H@W?xyljGrEiD)FULqeMUTPtj*udC0yr&Tj2 zLNVHaEhNu+All^p2;mT>B85muEcK{b`8z&isri%rhnJ(78qs5l-_D}^dPdPi;;WxG z+9of?zOj7oT7&I@+Uih096=>G${TyG6Bu@IUXo~{n54GvpPC-kCId|gF%|f5orD*e zGNaB$+_nOlUMF7#x2Z5ioL)kvyiyUTiw+Q3i^0zU0gmV&Q&mIziujU2tN)p3vdBR) zi1FVpuU5qAqczf!!ECb7V5UeNW&A+ShMu-JzbZf6JM3U^BK!}i?aMNLix=f-yl-7v z8KI!ds~*}O+oThOKDAkhvgpwQL}KP$NI0xgy^c$5y*leBwLVK%=u z&E5OyW&AWE>p_auDrlg-9%@8-xRNcFg$KzP zAAC3|$b7S*dzOqo)A92YO-qyGex3Ob&Ml;|%95PE))>0b%TpYM-Vge%?~2No!p>&n z9T(Dk-{jhw_t@e$XF9D19jV(aUzJa7xS1rc9mq2=5W*JrXf8nV&Oq{j9kIZU|EF&b an2R?P0~{_4x`O`+0Nyvyr`_wwss8}SQ=_r~ literal 0 HcmV?d00001 diff --git a/panther_lights/animations/battery_low.png b/panther_lights/animations/battery_low.png new file mode 100644 index 0000000000000000000000000000000000000000..bfdc182f7a0de0e2daec1a43e8042e632991050d GIT binary patch literal 2793 zcmYk84K&m1AHaXx7@Lqax|&O97SmeXG!l6?wkfZ9S?)qE^RnS)4WTAV5+jQ#O58={ zCGWI;ZTD70*D8fuGo@5mSLyZUKlgvm|NPH$zR%~J@7r^}=lgxm^PJd2^U_9cKmq`u zP4;&8S202*-I{RKp5#zC3;+;boSWMovYXpRR!Z{0xP%x0*j$oTvdjC{ZX;=Az-14s zA17QSrf&1mE`gH0jeB^=+TdRv-h4i`^_Q_Tb=@Rnv$}?}L8A^~q}QPvc~q`Z&{)X7 zSXlPyM*V%W-;d24>F@j4ouQ$5&q~%b$lfhWwB`Gp?D=-(2TooN@o0g$neC8(U-y&a zALF|+GY5Q?){Cjxr(Lf9yiJTljoRGmII*LkVMpSzB`0=Sc})2yA!oQw3+0?bXMZkA z72Si5o~nE@?8q+LXZ_*Y*sZ37cu9a{Tae5$@^(*C%2P#C(3_@Izwh84m&njrhkJT; z*9x%SYinyxBd=U5sR_bKU5_vGjlWNKMw(T)HY0zqH`sFw-ZQVUa_re(VbVK@9hPNk zUm}fOMxiy|La!d5$u!wXd!wdwhJ|p)BbiX!3w1ks!`^CYCd1Z`)2UBjg!G(RoSWT? z&gTu5=tA*V!>j<~f+;ABnUq|eC3FHpUt49tjEtb~P%$kT{W~8|PTq)h_~#$MsVt?G z*|=D&Y76{;ua`Tpvbt&p8u%&=g5@2W3IIBiRe_Y_bucQWMjDywq45%`tEF#rcf7&? z05oUG?k<6xiTUcGq)AWo@ofLPP2qbJ)DkOeLf>~d zw?AVM{-_;3iY*SF;!ww*hxkzAk$ELq;>ib8AV4 z&`X2Xtf@i`ZiI{RhZtIe+qtOHg#04^N4?h3^C7;BR;%H`Eh_NQXA= zvK&SITn3ADDvO+!nafGV7ZzqBX8(@1PQ|>i8K=+fFK?S2uK?u-EGYfJg}}MUP5Z)y1^n2BJ|D0GuN{PT7@2_3H_Ul9{PRm z1)4+J@*jrZZ7yp-oVncg+kUIj;xW#(;w$#=uh_@T{c|w^Z0;&Mu!VVpz{{H>hAlUl z7RH#4emvRkb9k#d+_0F$Wa!wO3$I?t0waQ$Ez&qMN*`~%{|2hI^a)PyWfu4rgG}v3 z!|TBg=y%Hk+q1-?+-xhwDJ-Rt$6OyvVT3_e&=Nym!7?JmYUDB40dEc!Zp({PkLV*s zA!QEJR5sycyzW*^si}TLIT0^^gXCfuov^{D{Xsp1R=ypdM9m9r5%oNJ=s;9XjQlF< zSR7!(*BY@P;MU?Ai333K7TQ6EgT{y% zEttV2b13XKEt!_`u_D@_{szwrjcX?kK#H-nLkup&I9xakzCWw{LGfTCC0QJGf=?G5 zS!e7bd<(uuI(Cq@YsxUneTwMy_2ayJbb|qdt8N@7+`yZ2o`y(8E`q1I`KbTX?ozeA zatbr#E_?*?RtSwG@s{3JiZsrAcr88nX6|HK)RO|)K7pnzMjtm_Uw*Qdl*lt|bVWd? zW(nu(SBpA@lF8OWG(M?m!1=+um5*M(T8E=y{-S_mKxeHZce|wwQ zO`n&DsDhh0eFiVPTPv8TgKOnZl2F}5aLrj(q~d`E1-R{+xo%w1^pUO7Rk89F{p3O$ z23&n%=HI~1GP3Dp4xa7wYUs@b&+w;dg4BNaMXf3&Y9T@xMCm}-W`gt+wk7#?47kRE zkMJLgvC1#amu)%1#!^)|k(n)|E}#!T(hwz|_c}Z;Qf1dza2ICDH&2%3Rq_lob(8TU z{ib)qF7}u)JRwg#g>hDwDV6^yZZ|gPtZ_hW!@#S8GGV15<7ABK!(IGphh=WCvXu}?{ND4-rVpX-(l`A zMf1O8WgFd0sVnZypEQ0~m2sF0B-k-L)sxI=vYQsezlan2S`%_?5w`JQ`bnAIe{FbL zz$*%P>H^D`ZR}G;75tg|6^*+@yafHYh33id^y}WrZbRBSur4T~(H34{RslayfEKF| zwCajd&9)_KAxt%}b-;p~unBxl?)GJ)wD@opY_#~$Zssa1R5NJIO>Vjhs`*vu?_k}^ zh?>7NazDmHYpN5&49NrU$4IML>&uac^IX-ZJT8O#YwrW;L85CRn*$Na8IoA%F&QMC zEs1p+OSR}PBI#@LbvPJy{8|frW#{TO>E%tH3@9Yd<#g#az!@nPQ=UQ}9vPuR7>}$m z6}*abtN?aA$^y$$1?pKOgH(ygThJj^fh;H!Q>7|&Ae;k*6Fjokl)JE!cc)tECJ|8t z55&594R(w%=ri0%xUL6SN8a)FX4xm zt#RutvcN9YgV3%z4)<+=iXDxu3czg~9gXGkLLaev1FAd%vr+M&r9P+YU8o zm4*oL1DR}sd+B&%{7}zXW*AQd|MU>W$yV<_2ec4!_^-_o`{}o@e++TpiJ+V%z5_4lj}$qsPPV_K6~7rAfONH0kl@-gbWhUVjOoIT6%4&!#oPo~lJAZ=X2s zBd_$DFnyJiF7EpivY3;83HB7S=&8~FKNRV3H2N_=D*r|EAK49?&8*&HUbLw`K>*o< L=HBf3^U?nRF*6D^ literal 0 HcmV?d00001 diff --git a/panther_lights/animations/strip01_purple.png b/panther_lights/animations/strip01_purple.png new file mode 100644 index 0000000000000000000000000000000000000000..bf4df0a71b0cec1551b846e2d49f89a58654699b GIT binary patch literal 964 zcmeAS@N?(olHy`uVBq!ia0vp^F+jY7gAGXLS#_UbU|?*?baoE#baqxKD9TUE%t>Wn z(3n^|(bnUzgUr$R;H9owvbP+MC_2AQaSS;UxH_PPYhhT%)q_ti>1SM;uw;h?*E&{q zd$HAgdgpdo?ci#9z}~UCd-9_0DL?ivyP$Ne`ElKz-QRaqvvZu)y1Zszp~K}|@8k-mg#)YteagBf&=uLoaK_ zpV+yk=-#6>p|95z`~N!>;20nI%<8P*k_{cI9s&y&^l`1)FmXYs`cbF0Z9!}!HAT7){FbZ@mg~5~#GIX%Keu*{ z9Yf?Rty*Ro8K#dysq3rvSl_sPdw;ss-@goYQ;H^KuYTVTj25;eZ+90U4Fo@(ch>_c z&H|6fVg?4jBOuH;Rhv&5D9B#o>Fdh=oQa>CLF3M~*6qNUJm~4-7*cWT?cK<{LjeLV z7tJKjC`|dQZ^CJOlSzIpSCv&cr~A@vj*c@|y$bEx>ck+kqk&ocLw`=(E0=?Z*yDa| z+^3;(u)K7^1eW{P`kaJng1KAYcv=RXm?y$repolA_dzw+&Hir(4zF*G=JM-mUsiMJ zi=3YNQBTHqOCl@^Dl?*X+++J5%k^R6!Sn~2A9!0;e(Xx)I?&Em9(3qIHkaoI(E{_D z53Rpv*Qg&obd}AHaeD!GxcUzl#vcbBu--Y;xLS9;#(_p|ivmV=y$6_#|5yKf>R;8s z&-*TZW28mF)+T#Rm4ovS=-xSWmGj?%gon({_OJX7dP-J3JaG7dIai)VL22Xtxl>Gj zOm!0a!zT0N!2@xi8?+0!+1H($T(m6Vq1>#ZpgAIzn)00gycMg1YNiXZ?vL0p`O!kn z_YcL^eLryitM5T|U-Ot+^{4Gl8!}f;p1+axbM)^ohfe!X)CGrK!2>2f>M?}=GVaTX Vh&=Qs{S7GcJzf1=);T3K0RXWBpEm#i literal 0 HcmV?d00001 diff --git a/panther_lights/animations/strip01_red.png b/panther_lights/animations/strip01_red.png new file mode 100644 index 0000000000000000000000000000000000000000..5886cfe1022b5657090ecdd55075c2cab06b0fb1 GIT binary patch literal 900 zcmeAS@N?(olHy`uVBq!ia0vp^F+jY7gAGXLS#_UbU|?*?baoE#baqxKD9TUE%t>Wn z(3n^|(bnUzgUr$R;H9owvbP+MC_2AQaSS;UxH_PPYhhT%)q_ti>1SM;uw;h?*E&{q zd$HAgdgpdo?ci#9z}~UCd-9_0DL?ivyP$Ne`ElKz-QRaqvvZu)y1Zszp~K}|@8k-mg#)YteagBf&=uLoaK_ zpV+yk=-#6>p|95z`~N!>;20nI%<8P*k_{cI9s&y&^l`1)FmXYs`cbF0Z9!}!HAT7){FbZ@mg~5~#GIX%Keu*{ z9Yf?Rty*Ro8K#dysq3rvSl_sPdw;ss-@goYQ;H^KuYTVTj25;eZ+91l|3DUlv3kvC zAjMhW5n0T@z;^_M8K-LVNdpDhOFVsD*`G7toiF*M zNgjybgRsPtruh;PJEZlv-;4b@!0=EQuZnH^mRYo{~G z?9gh;2deMqX|UE@#lA0%;SO)OZNdH%$jJ@9ACvna6CyxU|i!N3mWm$QM4 z-~PayO^h$bud9{2V)BDVTX#QP#cpTpkvl_a&xvU> r%HM7lmlMka`vB}1klU!jn0k@lcD264o%Ls~fg;b-)z4*}Q$iB}%Kv?@ literal 0 HcmV?d00001 diff --git a/panther_lights/animations/triangle01_blue.png b/panther_lights/animations/triangle01_blue.png new file mode 100644 index 0000000000000000000000000000000000000000..a5782194515485c4c298b2e67593b5273efe50ad GIT binary patch literal 1564 zcmV+%2IKjOP)EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|E{wGBU9JE&u=k8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1NBKnK~#9!?VVds6JZdBpDkldDJl|C(1;)h4IX0r`Cj@% z^uojh;~C{7WxeP$Y)D($?RS`MXOa!j-4gO-^7eh$q%D9FpJdZ=1P$%(dwF{gz0(t|0w}E?=?cN3+>$$(cFa3G~e0AW{Z}+kSeh$0>o&jrmz6AaO=as|u zi>TO!y{a#e+ZrP;fQOZqQYiGUf<98{cB4h?!fsc#dsjDmU18tQ>z4GfQ*A3hHD3Oh zg>DphKerzNPk;^G_Kw0{s9f98a~r@Hg={y^#pZVRH+v6wsT+P5xT$hnR7Clvp4(N> zXL{JobFl~e&XwHW*VtIp&2A0&&%~l$)2)C$(!*A?h%MOtzI`>fJNl;<1D;ptxz3P0 zx7E*$ZTPZo_r`$SZjAum(rc=DegOQ?7tmwh@ZlA3OJihbNNzI%dOy#1bi02ma645| z>_kbm0`95Yv{(zo=bYzlc@W&V^94hE$lk#9%9q&sDI)OucPjvqQ!>#OSK_1j?7KHP@;sjU%ku7WQPLE3w^tP>1l~k-w^bEovh8l7y4$LXGCu4kt9$IKIDVeXc(7+d z-D6k9@$!5O;IpLeF{M!%D?)lVT=2zYG zsK3m&x~Ervadl6t{^IJMR{h1*J+1nSt9x4Y7gzVR>MySDDb)X5^IVYL+tRtZr&0eF z;<=q7&(G++gJ>7&ZTF_qUM O0000EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|N*ME{L3h+W-In8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1ZYV_K~#9!?VVXq@=y?l-@+JEa6#jW5f|Lz7UR$N(jTH1 zCMFp71rV+MF6KiJL6-KMGliL?0cp8;GI@C)nxF?DgpRdIDFOwqz{SnvBp(SJN*rk4 z2PP)?Z{G(h72xGXfg6E6Jq^sx0*j0MTB!uw-}Cc?K&M2(US0+^Hh|4de*NMCxV&s! zmjLLLm>5}DVAv-o!0|Dko0{UsAOLE$L_n7h-1mXGxyE)Ei@@qCpL=@)o}T!zS_N)z z6?o+B&gB^J-X3st1nlktOG|t{41tdi;PbO_2>%cz(Xi*|`2(_8OyKq0UReRw z)__8Rw>_I>*y(iR+H9836$-%34MP^3=Ol8w>zlo^0~{XmhR@6Z{QdY5%uSp3||uj4bbH>AHt%AM8U52?bh7R zW&xtcW;c)L@wsfaeVz;J=dw0Dm*ed&6n^J+bCk3KPo?;p<~%QzfQJYE0IgJX!#h^M zjSY^Gt*!RC-EQj8>v_Jl#oPV-%)kXzMG_Mw%@uHGhvmlif9H0msbAy!{N73hxVsZq z72N~xQ~@)Y&bj^9rW5gdg;qs!zTJOSz;wEQ;OVsJsz^>@x2k(<1?&fQEA&oP(Q%%W zGuWNf{jUnxZ?tr(iVpMK4e)O2-mR7N0=!*S6p-iSWV?G+_bAg^6@}zES%=+^x z>Q7c-52EgIY`P%zx4YqksC!(|B1HYk+IB}#_qg-iq56}xD2b}>apk#V^*0YZPIZqf z&mF2i8Q<Z&N zl(?>dg4I7>;F2n!Q1z!+w_8#L6srCd8*CGIZ(QZMLG`CtV4J*qV>{0ctABLhrtjXk z$a910Pm#CV#@(ACQh$mB+veRHDe51wD$>BPC9AvOsyO(DOICNmRgnhT?j)$Y(5grS zMM+ZBU0_x04|o#QU07A5zP3Aw>MpD*QqQoHtnQ|(Vjp=h4heX=K&ik^0j}tGfgBr;%58 z%j!=fukM!BpH$uLsz0f^+f{#3b+@bjr0Q-#{XaiB&xt_D=T+TpsedX3(%iO?=d~LD zf@lzcP%HxL>zdo9^L%|Bcz*}s^pqoIc2@hg>3;6}@Njh%epf>x&F#47>+8o)wR)&j z!f((D+Z^=&;5`IE2+z|PF+E`jgdseHq5dBqo2^k1C~yU?qrerojsjQUItpBY5Bm^Q zG%W-F`cev3fmf?a!7A|UYo%Zn_}Q6KunPSATq#%_5BvLCQ>EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|E{v6KBHQF#rGn8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1M*2kK~#9!?VVds6JZdBpDklZDGCx%z=$9R4IX0r`Cj@% z^uojhgGUq)DfNOWSxJxE?=aiWB%4BaOURSS+xKCUQUD!%l1;-AIB*AUZf@&Q!;#v7 zZUT$?eG@nZjvaU$*h|0)@IaqC1itFm#|}I^>~&xpc%sk02R@XpJ9gmVV&uNUehs|R zb9eNy4V>2lx_#hH;9hCFcYsHF?lA56Zou<4d=O--0tpX?*lJ%!|wv~>c&OMljrr^o`OEt z!)Tt1IoL}#a{EAIV?j5&(c=#j3wlkv1o}h|W6>g}V0ZiW&D?J3Z(8(tU7_b%ee&E` zKex5v%evj0J#xD-0C+>MDd+hi@J-)9PkqDtSHNwJk=;JI%>d}#Jm1yr{;9wXRYfrq zCFKgZuX5Auk=y;IQEE7Ay47FoRmJ?k`&Gb&0l7VB(?wrvv?@yW?H*JC=PCf7vt1P> zE9})F6|kuAbGy*{RmIGCF4ggZX3XsR9~Re+jzXmMWl8^_O6O-x*M*F zQfa&EpzcPiqEw2Kx~RLssyH6-I;y*|swiV^cOBK;SXGpfVb@vRV^_s7@?1uOJq_v} zyDE;D=fiMt{??itlzW>VcVs=v&vx@S^< znQ3*;p#C!R>YiBrW#-jAvHFXvds_7uSNF8)FRt!s)n8oQ6R7`%=DC2d;p(1D{Tm>j z+X?dgoU2a|ZG*4_Y z0zY#IX@P%o2x)=8bqHyJzjFvFr{KVQO4EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|E{x6Zt{$zW@LL8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1dd5WK~#9!?VVds6Hyd}*XiuG^a2dMAYf3i3dUR1pYNl8 z!$jUpG>R8WOD{ux*i*r#7uuO~&K|l?(jdLW3 zEFQ`l=u!Yw^Za{h0CXeZB2wU1U~lJvS{Zmy<@>(2f%6XkJ1fvBOR)Dhf#VwRq{jDu zJ_pXu0bFDUI%TCscB>5g?LP2kk00AA^3P}l^#5c7x_IEF0BDrsbngJII&c{BW0wQq z_Z4v22d;*|_l^QjobGS~INS!FH-XoCz-a^6i#vQY0tOK8RkFYSTcJ{Jjm+}jVgfX5;5C~d|C^FWYe;Q#(E zVVHV3-ssVYJAB;doxZ$kAxW^udwa6CD+Pdpn`GZU#_(g68GSBopNmR(7_X8?Q+hjR zfGNCT;-u#%`uuAf_}K;8eV}`(37FeO}7*Yr75LM@QaN^bUO11S}T-%1!U>nMpVNnzG=jNWRlOZvqAj zs$^QV1V;8$B%iP+)qQdTE|c!tDG%mMMc;i+zF^N%_jwa=scM-s6@B)(7vPK3J-1|) z+;u8S=yURs?uF`}!v8Zl6{Yk!xrV)rx+gsn)t_9#UPav#CtaBOJ5Bg1>Yi4$NKt=s zmF^_!o_3%6RDW_+B}vsitv>gy{?>t~sqU%txo7n!+v!eI-P5QR-|9~`VK171F4HYV z{mB+=9}_SO>TfOKJ|6SDBrK&%r z1>45e8()2HQ~fCo*fy`;xbAb?>Yo_6?W;G=``ohnQ_^(1xO#J+)Sr^Tc6s$iiuxy= zinJTY`~F4O097ufef-EB|ByY~4);P*-0ZA`^w`<&KE-P4|mtMoaoo4Thp6<1Y% zT1R#Fss6N%>h4qhXtGf^Nr*&6%*XmE}uI{eYpH$u5sz0f^yH$Tub$6@& zr0Q;8{fF1w=ad8JU{lrIiTV%tF**#jw{7=%KjKdijYa^qYQWKs_O|UlKiUC$5kT)B z0<9W?1BKue z_?HWX;1u|WPYS^)@Q-H-!R0Y8Lj7KQr@(g$3fw7hK!K}i-sAHZ!Hw<<>rU!U00000 LNkvXXu0mjfeN9eo literal 0 HcmV?d00001 diff --git a/panther_lights/animations/triangle01_purple.png b/panther_lights/animations/triangle01_purple.png new file mode 100644 index 0000000000000000000000000000000000000000..54829b0914738ad3efdd1bdb12297e883a1b77b9 GIT binary patch literal 1752 zcmV;}1}FK6P)EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|N*LCIR0TCIA2c8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1hGj(K~#9!?VVds8&?!Y*NnHZ2V*Xd!56RtF@%b?QIwzE zpU}s?_NjlMeW;WwRRRG6V~maSfUyYR;LDtI_TaM-JRyXZK7I48j1&T3fIn*&dITQ0 z2d?dM6vvPmKs@Si#mJk^PU#(04T5uK%YL>bW;IcJ@8xDWy;j3vPYd3 zTQr6pe!)4X#9VMqhp&A0z~@bOk#)9c@S2}!@heRZDNzMrz<@s2+|Z*x{M7ys70s~M z*kpqW+q8JYJ9hb*GWF3)2?&PJ`&<#z;gTLbP8C;+XobB*#L6(;I~>sDkTNwQRuHlP zM66Jz#x+;8Ii}55VlId|SKH?*f#3D^CfihbO@$`wY?7ydkR>ED`sMQ!SZ9+8O-}fq zOWLaYTr{TpuD6@KWshIj;ea9=WXTZ{5)ceEIV4MtA{*>*z!e>Obm`Gm+vlPUc7e4A zy?sQ98hMrp$q?N8*NA|S5XiGki5h+S^y!WIyu+osY7tGa@AmfN-Y$?qM#;W^0Rb6= zh{BV7u5F*IN_dfVN>r%OeAL^)y()PK972TbcYS`$C)%9Q;hZnTKH=jgV4Xd7Ibxgr zCwu#;3x?%#*XP^p6LY~OXT+SVnu?;OO72g<25%`*B}eqAx5q=j?eiQFC8{)tIpd4E zspuT|_zAc?wzq$j^bA6tWwldLT&MfT6EHJ1@Q{q^sVFXCKUVjL6EK+i=iWgdHx*s? zxwwKoUfl~*SIM+f(Pf`I0sfr22hXgMNq|2!6*cs^xJdV;>ORjsoQhidTx`RhM&0L! zu222NChS?%J&B}iQh%!npGDo10Io^>#a6oKQTL?#+@<=9tty#U-IMBb*XnN^c#`U# zRG+(4e=(izB-K5cYH_RnViNZA6VM)PE$T0(V7r)rX;6P-33o98)1dxhBHb=0pmy~a z6IJ4R0%}(OM1gCXfLhgGl1#Uz38+>5B`Me@-rl(CbCc>XNx(Mw_QrOfn^yn1ft!AN zMx6^?k?3|7E#?@s=q9%y1P<;S!8v0q5iVy z>TX;8Wzp5$w)%@#cf0B@Ufu1gzj$@GtN!BE-G=)2=+PyEAg`!EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|E{uE)<8u;{X5v8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1Mx{jK~#9!?VVds6JZdBpDkldDGCx%z=$9R4IX0r`Cj@% z^uojhgGUq)DfPl>vXUNmzr$=hlWc(QmXIftx9`(UN&)onNj5D<;J_WYwYjB76Gxf` zx(zJp&u!ogIC0={U@rr!z7DkpE&UFus47m;ITgb4tQU>?!29jUE9%fTfirUY&p-x;&%5p`v7>Z8-544scKx5H2J2U+gH#R zdRWbKu?BnjT5ca|Y%J<#w+6f~v8dN{OQ28nuof+133k74U(fB1{-MQy=M{RcGbGQg z^>b4jzM|W`H6XWJBY?N`nsS~W0blh6^vpMWcm>?i7}*<=+l+wT&+|Rq?jH)=QdJZy zQBtme2P!x10l7VFTBU}wu3P=tU{%Zxd{_ls9Fg0jHeK|&R;!}q-tJKqaG?V51>;pw za>8C4Qvr(#zqb#4SXIoO=aLKdFm>-#h>~fmV$M9z0(_jhw*mkkR241cx#ZaHN!6VI z(5>aUq=r3>y4M|x>Mto_S5fy6n{G+{lWuqwbq^ISR@7foZFe1Y51r>Z)n8IYNnLdh zmFKzDKRWP`>K-c3bE?0@-tLg<9s+o7^_Q5i$5p_Tu&t=S#Dblp0yaVYqiuMO3fKho zml)ffvjSRIe~A$#xhtS$^$!Z%QU$cC{t|M#Emc6P>Mx;S$GCfwE6-!9zl4At^X^UR zJddsZwSmXJdy^y2W2(Q@-R=~3Z)QpTr4H)6{iB;M0K}T6=j<3ZlbzdtBNuy>?W&w?5a3Tp35Y#XF=U# zSH(&5d@S%;Qui2DaoRkWc~bY#Rk2E*%e<+3sH#|1{be52J*WE1JgR$6^_O{7_gv~P z^Q`VU)L-UZ-BYW-%)7d$R)2AIPpkgo>Yi5p#nnBn`irZ3eD%N7JQqMVUEPzZe+v-L z?f7|q!M{%s?E&LHU&hyH$e+6_N;25C1O-*7k0S)RXgPmze&&eaqE0000< KMNUMnLSTYEB<>{u literal 0 HcmV?d00001 diff --git a/panther_lights/animations/triangle01_yellow.png b/panther_lights/animations/triangle01_yellow.png new file mode 100644 index 0000000000000000000000000000000000000000..5c478584fd1cd64b98aff69bdab7f4043d3325e0 GIT binary patch literal 1676 zcmV;726Op|P)EX>4Tx04R}tkv&MmKpe$iQ>9X>4t5Z6$j~}j5EXIMDionYs1;guFuC*#ni!H4 z7e~Rh;NZt%)xpJCR|i)?5c~jfb#YR3krMxx6k5c1aNLh~_a1le0HIN3n$;;BvB z;Ji;9Wo204LKlt6PRh$_2k|If<`^9lS;y`E@Xx1I)``B@sCqVESxYFDHjRr9NNqW7l z#g2gfZQ$a%ttorJh;#z$LRx*rLNL9z`-Ff zQljiNpLh3k_V(|YR)0ShgmQw5@%E|!000JJOGiWi{{a60|De66lK=n!32;bRa{vG? zBLDy{BLR4&KXw2B00(qQO+^Ri0|N*M4J!NdsQ>@~8FWQhbVF}#ZDnqB07G(RVRU6= zAa`kWXdp*PO;A^X4i^9b1Z7D?K~#9!?VVXq@=y?l-@+JEa3yYFME!7!TZ}*7OMi%7 zn3!O2UqGbxyO=Z}iURF9X9_b(1JZKyWb*PpG(itQ2pwyaQ3MKHflHg|X+F|8ls3@5 z4@^z*-@Xsj>%jA~0yhGCb`~fUfaPU=ty%@{?)Z6Hpi`P)7mL8=Ch+eczkYE6Twb=X zO9OODON=ZnG3=8Q;P{x&&CKv)5CF|)I-rXO?)$*PLVLR_6<}?R&%M3^kB|J=XaKdE z0*}4jb8`%MZx1*+0(N(Sl@&f8hQP-M&}y|0=^vsb8TR5Le?V3$93uw@K&jMjDW1pB zgMdN5y)ksj(Lz#T`+j@7x3_t-i$#V#F~QeOP4R2*@7z`%9ynfJUgSa-2|UW})m319 z9VnN1+w*yboz1qd&FA@CxeVOgFl5PjP7=2}+U%Vj;P8+)e10C7oCGo%juL>MPfqf= z?QI6#Z1N#B&q*5W+1anTy}!?~k<0OBXF9%&h@R(id`%FvLBGB6AuU=+66~mNcjtCK z4-mCCyK_8`&*k$!=ee|gE^5Q)=6Jiypy$k)%XPX9e8ZVY%`B@44M;qHBDg-&?N(x3}`D zqI2NADqt?xJGXz^bRvGQ)T&6XxBIsWn9U9jJe!qW70D&+ZguajfP=v9hTf|xy3TWQ z1-qBJ|5gD9jh0?j(Pf@H0sfb||J6$R0sd1}l#u7-V!QiQ_c+sC6{X}k*@iucy2l-f z>Q6Rd52Nl$Y`P@%x4PlOsC!b;B1Qel)^^8H_oVaOrTUYtD2c1?N#(g~^*0VYNp(*u z&t0lNncnUs)jbK|uGODR!v3oQ+Jr4d{mB$;7ZorK>ThhrT~xp{s6Uz5ZkH8My84re zC~;i@C98j;z$H~csp?NjZnvZgC{_I_DcC0N-nhzhlj=`Nz&3gJ#&({YR{z+*P2at7 zk>@7WpW<$}jk`Car2Z5Kw#~aYQq(_gRit5IOICNuRdMJIm#prRt0E1v-DyyFsa27N ziITLayTqzE5b!jryR@oEgKT#i)m>Uuq<&$iS=~)n#X<6%`hh(P>TbF!_M7K_0v{!H zH&GP_&2t(jbx*n~4wL6JZt9*?RUB6RX&lwvrTWu2s=G_|r*TzxSL#pWtnMz4K}&xrskl~mnrsedK|kmk1eJa0Dn z7es>qph^YU*wEZIo#z`H{6R{mryMDTg7$6G{oMBf@9GL+R1JwVx09aF&#yn#>ecHA zBhU)l9P|jh7X%1BPh-UNgdrbZ80!D=v057yfdW_HItpBY>nLyquA{&e_^1zs>X~+d ze|{(B--lb>@LEkFSlw_yH(X6~gy%m4 W-WudFTJW0y0000 +#include +#include +#include +#include +#include + +#include + +namespace panther_lights +{ + +class Animation +{ +public: + virtual ~Animation() {} + + /** + * @brief Initialize and verify if animation was correctly defined + * + * @param animation_description YAML description of animation + * @param num_led number of LEDs + * @param controller_frequency frequency at which animation will be updated + * + * @exception std::out_of_range or std::runtime_error if animation + * parameters defined in description are missing or are incorrect + */ + virtual void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) + { + Reset(); + num_led_ = num_led; + + if (!animation_description["duration"]) { + throw std::runtime_error("Missing 'duration' in animation description"); + } + + auto duration = animation_description["duration"].as(); + if ((duration - std::numeric_limits::epsilon()) <= 0.0) { + throw std::out_of_range("Duration has to be positive"); + } + + if (animation_description["repeat"]) { + loops_ = animation_description["repeat"].as(); + } + + if (duration * loops_ > 10.0) { + throw std::runtime_error("Animation display duration (duration * repeat) exceeds 10 seconds"); + } + + if (animation_description["brightness"]) { + auto brightness = animation_description["brightness"].as(); + if (brightness < 0.0 || brightness > 1.0) { + throw std::out_of_range("Brightness has to be in range <0,1>"); + } + brightness_ = static_cast(round(brightness * 255)); + } + + anim_len_ = int(round(duration * controller_frequency)); + full_anim_len_ = anim_len_ * loops_; + + if (anim_len_ < 1) { + throw std::runtime_error( + "Animation duration is too short to display with the current frequency"); + } + } + + /** + * @brief Update and return animation frame + * + * @returns the newest animation frame, if animation is finished will return vector filled with 0 + * @exception std::runtime_error if UpdateFrame() method returns frame with invalid size + */ + std::vector Call() + { + if (current_cycle_ < loops_) { + auto frame = UpdateFrame(); + + if (frame.size() != num_led_ * 4) { + throw std::runtime_error( + "Invalid frame size. Check animation UpdateFrame() method implementation"); + } + + anim_iteration_++; + progress_ = float(anim_iteration_ + anim_len_ * current_cycle_) / full_anim_len_; + + if (anim_iteration_ >= anim_len_) { + anim_iteration_ = 0; + current_cycle_++; + } + + if (current_cycle_ >= loops_) { + finished_ = true; + } + + return frame; + } + + return std::vector(num_led_ * 4, 0); + } + + /** + * @brief Reset animation, this will cause animation + * to start from the beginning if Update() method is invoked + */ + void Reset() + { + anim_iteration_ = 0; + current_cycle_ = 0; + finished_ = false; + progress_ = 0.0; + } + + bool IsFinished() const { return finished_; } + std::size_t GetNumberOfLeds() const { return num_led_; } + std::uint8_t GetBrightness() const { return brightness_; } + float GetProgress() const { return progress_; } + + virtual void SetParam(const std::string & /*param*/){}; + +protected: + Animation() {} + + /** + * @brief Abstract method that has to be implemented inside child class + * it should return RGBA animation frame with size equal to num_led_ * 4 + */ + virtual std::vector UpdateFrame() = 0; + + std::size_t GetAnimationLength() const { return anim_len_; } + std::size_t GetAnimationIteration() const { return anim_iteration_; } + +private: + std::size_t num_led_; + std::size_t anim_len_; + std::size_t full_anim_len_; + + bool finished_ = false; + float progress_ = 0.0; + std::size_t loops_ = 1; + std::size_t current_cycle_ = 0; + std::size_t anim_iteration_ = 0; + std::uint8_t brightness_ = 255; + + std::string param_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/animation/charging_animation.hpp b/panther_lights/include/panther_lights/animation/charging_animation.hpp new file mode 100644 index 000000000..3a9c3f377 --- /dev/null +++ b/panther_lights/include/panther_lights/animation/charging_animation.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ +#define PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ + +#include +#include +#include +#include + +#include + +#include + +namespace panther_lights +{ + +class ChargingAnimation : public Animation +{ +public: + ChargingAnimation() {} + ~ChargingAnimation() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override; + + void SetParam(const std::string & param) override; + +protected: + std::vector UpdateFrame() override; + + std::array HSVtoRGB(const float h, const float s, const float v) const; + std::vector CreateRGBAFrame( + const std::array & color, const float brightness) const; + +private: + static constexpr float kFadeFactor = 0.15; + static constexpr float kHMin = 0.0; + static constexpr float kHMax = 120.0; + + std::size_t fade_duration_; + std::size_t fill_start_; + std::size_t fill_end_; + std::array color_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/animation/image_animation.hpp b/panther_lights/include/panther_lights/animation/image_animation.hpp new file mode 100644 index 000000000..5f50e4a27 --- /dev/null +++ b/panther_lights/include/panther_lights/animation/image_animation.hpp @@ -0,0 +1,82 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_IMAGE_ANIMATION_HPP_ +#define PANTHER_LIGHTS_IMAGE_ANIMATION_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + +#include + +namespace gil = boost::gil; + +namespace panther_lights +{ + +class ImageAnimation : public Animation +{ +public: + ImageAnimation() {} + ~ImageAnimation() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override; + +protected: + std::vector UpdateFrame() override; + + /** + * @brief Process raw image path including extracting ros package shared directory path specified + * with '$(find ros_package)` syntax + * + * @param image_path raw path to an image, it should be a global path or should contain '$(find + * ros_package)` syntax + * + * @returns global path to an image file + * @exception std::runtime_error if provided image_path is invalid or file does not exists + */ + std::filesystem::path ParseImagePath(const std::string & image_path) const; + + gil::rgba8_image_t RGBAImageResize( + const gil::rgba8_image_t & image, const std::size_t width, const std::size_t height) const; + + /** + * @brief This method converts RGB image to gray, normalizes gray image brightness and then + * applies provided color + * + * @param image RGB image that will be converted + * @param color 24-bit RGB color + */ + void RGBAImageConvertColor(gil::rgba8_image_t & image, const std::uint32_t color) const; + + gil::gray_alpha8_image_t RGBAImageConvertToGrey(const gil::rgba8_image_t & image) const; + + void GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & image) const; + +private: + gil::rgba8_image_t image_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_IMAGE_ANIMATION_HPP_ diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp new file mode 100644 index 000000000..1c88b8257 --- /dev/null +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ +#define PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ + +#include +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +class ControllerNode : public rclcpp::Node +{ +public: + ControllerNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~ControllerNode() {} + +private: + std::shared_ptr> animation_loader_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ diff --git a/panther_lights/package.xml b/panther_lights/package.xml index 7788b31fa..736ad3dab 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -17,8 +17,10 @@ ament_cmake image_transport + libpng-dev panther_gpiod panther_msgs + pluginlib rclcpp sensor_msgs diff --git a/panther_lights/plugins.xml b/panther_lights/plugins.xml new file mode 100644 index 000000000..11f0ed750 --- /dev/null +++ b/panther_lights/plugins.xml @@ -0,0 +1,8 @@ + + + Animation processed from an image + + + Charging animation representing percentage of battery + + diff --git a/panther_lights/src/animation/charging_animation.cpp b/panther_lights/src/animation/charging_animation.cpp new file mode 100644 index 000000000..411c491bd --- /dev/null +++ b/panther_lights/src/animation/charging_animation.cpp @@ -0,0 +1,160 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace panther_lights +{ + +void ChargingAnimation::Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) +{ + Animation::Initialize(animation_description, num_led, controller_frequency); + + fade_duration_ = int(std::round(this->GetAnimationLength() * kFadeFactor)); +} + +void ChargingAnimation::SetParam(const std::string & param) +{ + float battery_percent; + try { + battery_percent = std::clamp(std::stof(param), 0.0f, 1.0f); + } catch (const std::invalid_argument & /*e*/) { + throw std::invalid_argument("Can not cast param to float!"); + } + + const auto anim_len = this->GetAnimationLength(); + std::size_t on_duration = static_cast(std::round(float(anim_len) * battery_percent)); + + on_duration = on_duration < 2 * fade_duration_ ? 2 * fade_duration_ : on_duration; + + if (on_duration >= anim_len) { + fade_duration_ = 0; + } + + fill_start_ = (anim_len - on_duration) / 2; + fill_end_ = (anim_len + on_duration) / 2; + const float hue = (kHMax - kHMin) * battery_percent + kHMin; + color_ = HSVtoRGB(hue / 360.0, 1.0, 1.0); +} + +std::vector ChargingAnimation::UpdateFrame() +{ + auto anim_iteration = this->GetAnimationIteration(); + auto empty_frame = std::vector(this->GetNumberOfLeds() * 4, 0); + for (std::size_t i = 3; i < empty_frame.size(); i += 4) { + empty_frame[i] = 255; + } + + if (anim_iteration < fill_start_) { + return empty_frame; + } + + if (anim_iteration < fill_start_ + fade_duration_) { + auto brightness = std::sin(M_PI_2 * (float(anim_iteration - fill_start_) / fade_duration_)); + return CreateRGBAFrame(color_, brightness); + } + + if (anim_iteration <= fill_end_ - fade_duration_) { + return CreateRGBAFrame(color_, 1.0); + } + + if (anim_iteration < fill_end_) { + auto brightness = std::sin( + M_PI / (2.0 * fade_duration_) * + ((float(anim_iteration) - float(fill_end_)) + 2.0 * fade_duration_)); + return CreateRGBAFrame(color_, brightness); + } + + return empty_frame; +} + +std::array ChargingAnimation::HSVtoRGB( + const float h, const float s, const float v) const +{ + if (std::fabs(s) < std::numeric_limits::epsilon()) { + return { + {static_cast(v * 255), static_cast(v * 255), + static_cast(v * 255)}}; + } + + const int i = static_cast(std::floor(h * 6.0f)); + const float f = (h * 6.0f) - i; + const float p = v * (1.0f - s); + const float q = v * (1.0f - s * f); + const float t = v * (1.0f - s * (1.0f - f)); + + switch (i % 6) { + case 0: + return { + {static_cast(v * 255), static_cast(t * 255), + static_cast(p * 255)}}; + case 1: + return { + {static_cast(q * 255), static_cast(v * 255), + static_cast(p * 255)}}; + case 2: + return { + {static_cast(p * 255), static_cast(v * 255), + static_cast(t * 255)}}; + case 3: + return { + {static_cast(p * 255), static_cast(q * 255), + static_cast(v * 255)}}; + case 4: + return { + {static_cast(t * 255), static_cast(p * 255), + static_cast(v * 255)}}; + default: + return { + {static_cast(v * 255), static_cast(p * 255), + static_cast(q * 255)}}; + } +} + +std::vector ChargingAnimation::CreateRGBAFrame( + const std::array & color, const float brightness) const +{ + const std::array rgba_color = { + static_cast(color[0] * brightness), + static_cast(color[1] * brightness), + static_cast(color[2] * brightness), + 255, + }; + std::vector frame(this->GetNumberOfLeds() * rgba_color.size()); + + for (std::size_t i = 0; i < frame.size(); i += rgba_color.size()) { + std::copy(rgba_color.begin(), rgba_color.end(), frame.begin() + i); + } + + return frame; +} + +} // namespace panther_lights + +#include + +PLUGINLIB_EXPORT_CLASS(panther_lights::ChargingAnimation, panther_lights::Animation) diff --git a/panther_lights/src/animation/image_animation.cpp b/panther_lights/src/animation/image_animation.cpp new file mode 100644 index 000000000..b80d34707 --- /dev/null +++ b/panther_lights/src/animation/image_animation.cpp @@ -0,0 +1,164 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace panther_lights +{ + +void ImageAnimation::Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) +{ + Animation::Initialize(animation_description, num_led, controller_frequency); + + if (!animation_description["image"]) { + throw std::runtime_error("No 'image' in animation description"); + } + + const auto image_path = ParseImagePath(animation_description["image"].as()); + gil::rgba8_image_t base_image; + gil::read_and_convert_image(std::string(image_path), base_image, gil::png_tag()); + image_ = RGBAImageResize(base_image, this->GetNumberOfLeds(), this->GetAnimationLength()); + + if (animation_description["color"]) { + RGBAImageConvertColor(image_, animation_description["color"].as()); + } +} + +std::vector ImageAnimation::UpdateFrame() +{ + std::vector frame; + for (std::size_t i = 0; i < this->GetNumberOfLeds(); i++) { + auto pixel = gil::const_view(image_)(i, this->GetAnimationIteration()); + frame.push_back(pixel[0]); + frame.push_back(pixel[1]); + frame.push_back(pixel[2]); + frame.push_back(pixel[3]); + } + + return frame; +} + +std::filesystem::path ImageAnimation::ParseImagePath(const std::string & image_path) const +{ + std::filesystem::path global_img_path; + + if (!std::filesystem::path(image_path).is_absolute()) { + if (image_path[0] != '$') { + throw std::runtime_error("Invalid image path"); + } + + std::smatch match; + if (!std::regex_search(image_path, match, std::regex("^\\$\\(find .*\\)"))) { + throw std::runtime_error("Can't process substitution expression"); + } + + const std::string ros_pkg_expr = match[0]; + const std::string ros_pkg = std::regex_replace( + ros_pkg_expr, std::regex("^\\$\\(find \\s*|\\)$"), ""); + const std::string img_relative_path = image_path.substr(ros_pkg_expr.length()); + + try { + global_img_path = ament_index_cpp::get_package_share_directory(ros_pkg) + img_relative_path; + } catch (const ament_index_cpp::PackageNotFoundError & e) { + throw std::runtime_error("Can't find ROS package: " + ros_pkg); + } + } else { + global_img_path = image_path; + } + + if (!std::filesystem::exists(global_img_path)) { + throw std::runtime_error("File doesn't exists: " + std::string(global_img_path)); + } + + return global_img_path; +} + +gil::rgba8_image_t ImageAnimation::RGBAImageResize( + const gil::rgba8_image_t & image, const std::size_t width, const std::size_t height) const +{ + gil::rgba8_image_t resized_image(width, height); + gil::resize_view(gil::const_view(image), view(resized_image), gil::bilinear_sampler()); + + return resized_image; +} + +void ImageAnimation::RGBAImageConvertColor( + gil::rgba8_image_t & image, const std::uint32_t color) const +{ + auto grey_image = RGBAImageConvertToGrey(image); + GreyImageNormalizeBrightness(grey_image); + + // extract RGB values from hex + auto r = (std::uint32_t(color) >> 16) & (0xFF); + auto g = (std::uint32_t(color) >> 8) & (0xFF); + auto b = (std::uint32_t(color)) & (0xFF); + + gil::transform_pixels( + gil::const_view(grey_image), gil::view(image), + [r, g, b](const gil::gray_alpha8_pixel_t & pixel) { + return gil::rgba8_pixel_t( + static_cast(pixel[0] * r / 255), + static_cast(pixel[0] * g / 255), + static_cast(pixel[0] * b / 255), pixel[1]); + }); +} + +gil::gray_alpha8_image_t ImageAnimation::RGBAImageConvertToGrey( + const gil::rgba8_image_t & image) const +{ + gil::gray_alpha8_image_t grey_image(image.dimensions()); + gil::transform_pixels( + gil::const_view(image), gil::view(grey_image), [](const gil::rgba8_pixel_t & pixel) { + return gil::gray_alpha8_pixel_t( + static_cast(0.299 * pixel[0] + 0.587 * pixel[1] + 0.114 * pixel[2]), + pixel[3]); + }); + return grey_image; +} + +void ImageAnimation::GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & image) const +{ + std::uint8_t max_value = *std::max_element( + gil::nth_channel_view(gil::const_view(image), 0).begin(), + gil::nth_channel_view(gil::const_view(image), 0).end()); + gil::transform_pixels( + gil::const_view(image), gil::view(image), [max_value](const gil::gray_alpha8_pixel_t & pixel) { + return gil::gray_alpha8_pixel_t( + static_cast(float(pixel[0]) / float(max_value) * 255), pixel[1]); + }); +} + +} // namespace panther_lights + +#include + +PLUGINLIB_EXPORT_CLASS(panther_lights::ImageAnimation, panther_lights::Animation) diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp new file mode 100644 index 000000000..b13b79c07 --- /dev/null +++ b/panther_lights/src/controller_node.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + animation_loader_ = std::make_shared>( + "panther_lights", "panther_lights::Animation"); + + YAML::Node anim_desc = YAML::LoadFile("/home/ros/ros2_ws/src/test.yaml"); + + try { + std::shared_ptr image_anim = + animation_loader_->createSharedInstance("panther_lights::ImageAnimation"); + int num_led = 46; + image_anim->Initialize(anim_desc, num_led, 47.0); + + auto pub = this->create_publisher("frame", 10); + sensor_msgs::msg::Image image; + image.header.frame_id = "frame_id"; + // image.header.stamp = rospy.Time.now(); + image.encoding = "rgba8"; + image.height = 1; + image.width = num_led; + image.step = 4 * num_led; + + while (rclcpp::ok()) { + if (image_anim->IsFinished()) { + image_anim->Reset(); + } + auto a = image_anim->Call(); + image.data = a; + pub->publish(image); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + // for (auto & item : a) { + // std::cout << unsigned(item) << ", "; + // } + // std::cout << "]]]" << std::endl; + // for (auto & item : image.data) { + // std::cout << unsigned(item) << ", "; + // } + // std::cout << "]]]" << std::endl; + } + + auto a = image_anim->Call(); + // for (auto & item : a) { + // std::cout << unsigned(item) << ", "; + // } + // std::cout << std::endl; + + } catch (pluginlib::PluginlibException & e) { + printf("The plugin failed to load. Error: %s\n", e.what()); + } + + RCLCPP_INFO(this->get_logger(), "Node started"); +} + +} // namespace panther_lights diff --git a/panther_lights/src/controller_node_main.cpp b/panther_lights/src/controller_node_main.cpp new file mode 100644 index 000000000..9f747366e --- /dev/null +++ b/panther_lights/src/controller_node_main.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto lights_controller_node = + std::make_shared("lights_controller_node"); + + try { + rclcpp::spin(lights_controller_node); + } catch (const std::runtime_error & err) { + std::cerr << "[lights_controller_node] Caught exception: " << err.what() << std::endl; + } + + std::cout << "[lights_controller_node] Shutting down" << std::endl; + rclcpp::shutdown(); + return 0; +} diff --git a/panther_lights/test/test_animation.cpp b/panther_lights/test/test_animation.cpp new file mode 100644 index 000000000..6035f8003 --- /dev/null +++ b/panther_lights/test/test_animation.cpp @@ -0,0 +1,194 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include + +#include + +class AnimationWrapper : public panther_lights::Animation +{ +public: + AnimationWrapper() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override + { + Animation::Initialize(animation_description, num_led, controller_frequency); + frame_size_ = this->GetNumberOfLeds() * 4; + } + + std::vector UpdateFrame() override + { + return std::vector(frame_size_, 147); + } + + std::size_t GetAnimationLength() const { return Animation::GetAnimationLength(); } + std::size_t GetAnimationIteration() const { return Animation::GetAnimationIteration(); } + void SetFrameSize(const std::size_t frame_size) { frame_size_ = frame_size; } + +private: + std::size_t frame_size_; +}; + +class TestAnimation : public testing::Test +{ +public: + TestAnimation() { animation_ = std::make_unique(); } + ~TestAnimation() {} + +protected: + std::unique_ptr animation_; +}; + +TEST_F(TestAnimation, Initialize) +{ + YAML::Node animation_description; + + // missing duration + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + // invalid duration + animation_description["duration"] = "-1.0"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); + animation_description["duration"] = "word"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), YAML::BadConversion); + + // invalid animation length + animation_description["duration"] = "0.1"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 1.0), std::runtime_error); + + animation_description["duration"] = "2.0"; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); + + // invalid repeat + animation_description["repeat"] = "-2"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), YAML::BadConversion); + animation_description["repeat"] = "1.1"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), YAML::BadConversion); + + // exceeded anim display duration + animation_description["repeat"] = "6"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + animation_description["repeat"] = "2"; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); + + // invalid brightness + animation_description["brightness"] = "-0.5"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); + animation_description["brightness"] = "1.2"; + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); + + animation_description["brightness"] = "0.5"; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); +} + +TEST_F(TestAnimation, CheckInitialValues) +{ + const std::size_t num_led = 10; + const float controller_frequency = 10.0; + YAML::Node animation_description = YAML::Load("{duration: 2.0, repeat: 2}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, controller_frequency)); + + EXPECT_EQ(num_led, animation_->GetNumberOfLeds()); + EXPECT_EQ(std::size_t(20), animation_->GetAnimationLength()); + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_FALSE(animation_->IsFinished()); + EXPECT_FLOAT_EQ(0.0, animation_->GetProgress()); +} + +TEST_F(TestAnimation, Reset) +{ + ASSERT_NO_THROW(animation_->Initialize(YAML::Load("{duration: 2.0}"), 10, 10.0f)); + animation_->Call(); + EXPECT_NO_THROW(animation_->Call()); + + // call animation + ASSERT_NO_THROW(animation_->Call()); + EXPECT_NE(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_NE(0.0F, animation_->GetProgress()); + + // reset animation + animation_->Reset(); + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_FLOAT_EQ(0.0, animation_->GetProgress()); + EXPECT_FALSE(animation_->IsFinished()); +} + +TEST_F(TestAnimation, CallWithInvalidFrameSize) +{ + ASSERT_NO_THROW(animation_->Initialize(YAML::Load("{duration: 2.0}"), 10, 10.0f)); + EXPECT_NO_THROW(animation_->Call()); + + animation_->SetFrameSize(11); + EXPECT_THROW(animation_->Call(), std::runtime_error); +} + +TEST_F(TestAnimation, Call) +{ + const std::size_t num_led = 10; + const float controller_frequency = 10.0; + YAML::Node animation_description = YAML::Load("{duration: 2.0, repeat: 2}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, controller_frequency)); + + // check random progress + for (std::size_t i = 0; i < 5; i++) { + ASSERT_NO_THROW(animation_->Call()); + } + EXPECT_EQ(std::size_t(5), animation_->GetAnimationIteration()); + EXPECT_FALSE(animation_->IsFinished()); + float expected_progress = 5.0 / (20 * 2); + EXPECT_FLOAT_EQ(expected_progress, animation_->GetProgress()); + + animation_->Reset(); + + // reach end of first loop of animaiton + for (std::size_t i = 0; i < 20; i++) { + ASSERT_NO_THROW(animation_->Call()); + } + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_FALSE(animation_->IsFinished()); + expected_progress = 20.0 / (20 * 2); + EXPECT_FLOAT_EQ(expected_progress, animation_->GetProgress()); + + // reach animaiton end + for (std::size_t i = 0; i < 20; i++) { + ASSERT_NO_THROW(animation_->Call()); + } + EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); + EXPECT_TRUE(animation_->IsFinished()); + EXPECT_FLOAT_EQ(1.0, animation_->GetProgress()); + + // after reaching animaiton end Call() method when invoked should return frame filled with 0 + auto frame = animation_->Call(); + EXPECT_EQ(num_led * 4, frame.size()); + for (std::size_t i = 0; i < num_led * 3; i++) { + EXPECT_EQ(0, frame[i]); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_charging_animation.cpp b/panther_lights/test/test_charging_animation.cpp new file mode 100644 index 000000000..e5f685a3e --- /dev/null +++ b/panther_lights/test/test_charging_animation.cpp @@ -0,0 +1,201 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +#include + +class ChargingAnimationWrapper : public panther_lights::ChargingAnimation +{ +public: + ChargingAnimationWrapper() {} + + std::vector UpdateFrame() { return ChargingAnimation::UpdateFrame(); } + + std::array HSVtoRGB(const float h, const float s, const float v) const + { + return ChargingAnimation::HSVtoRGB(h, s, v); + } + + std::vector CreateRGBAFrame( + const std::array color, const float brightness) const + { + return ChargingAnimation::CreateRGBAFrame(color, brightness); + } + + std::size_t GetAnimationLength() const { return ChargingAnimation::GetAnimationLength(); } + std::size_t GetAnimationIteration() const { return ChargingAnimation::GetAnimationIteration(); } +}; + +class TestChargingAnimation : public testing::Test +{ +public: + TestChargingAnimation(); + ~TestChargingAnimation(); + +protected: + void TestRGBColor( + const std::array & color, std::uint8_t r, std::uint8_t g, + std::uint8_t b) const; + void TestRGBAFrame( + const std::vector & frame, std::uint8_t r, std::uint8_t g, std::uint8_t b, + std::uint8_t a); + + std::unique_ptr animation_; +}; + +TestChargingAnimation::TestChargingAnimation() +{ + animation_ = std::make_unique(); +} + +TestChargingAnimation::~TestChargingAnimation() {} + +void TestChargingAnimation::TestRGBColor( + const std::array & color, std::uint8_t r, std::uint8_t g, std::uint8_t b) const +{ + EXPECT_EQ(r, color[0]); + EXPECT_EQ(g, color[1]); + EXPECT_EQ(b, color[2]); +} + +void TestChargingAnimation::TestRGBAFrame( + const std::vector & frame, std::uint8_t r, std::uint8_t g, std::uint8_t b, + std::uint8_t a) +{ + for (std::size_t i = 0; i < frame.size(); i += 4) { + EXPECT_EQ(r, frame[i]); + EXPECT_EQ(g, frame[i + 1]); + EXPECT_EQ(b, frame[i + 2]); + EXPECT_EQ(a, frame[i + 3]); + } +} + +TEST_F(TestChargingAnimation, HSVtoRGB) +{ + // saturation equal 0 with max value should return white + auto color = animation_->HSVtoRGB(0.0, 0.0, 1.0); + TestRGBColor(color, 255, 255, 255); + + // red + color = animation_->HSVtoRGB(0.0, 1.0, 1.0); + TestRGBColor(color, 255, 0, 0); + + // green + color = animation_->HSVtoRGB(120.0 / 360.0, 1.0, 1.0); + TestRGBColor(color, 0, 255, 0); + + // blue + color = animation_->HSVtoRGB(240.0 / 360.0, 1.0, 1.0); + TestRGBColor(color, 0, 0, 255); + + // test colors for each of 6 hue segments + color = animation_->HSVtoRGB(51.0f / 360.0f, 0.8f, 0.3f); + TestRGBColor(color, 76, 67, 15); + + color = animation_->HSVtoRGB(77.0f / 360.0f, 0.5f, 0.7f); + TestRGBColor(color, 153, 178, 89); + + color = animation_->HSVtoRGB(150.0f / 360.0f, 0.2f, 0.8f); + TestRGBColor(color, 163, 204, 183); + + color = animation_->HSVtoRGB(222.0f / 360.0f, 0.8f, 0.5f); + TestRGBColor(color, 25, 56, 127); + + color = animation_->HSVtoRGB(291.0f / 360.0f, 0.7f, 0.4f); + TestRGBColor(color, 91, 30, 102); + + color = animation_->HSVtoRGB(345.0f / 360.0f, 0.1f, 0.9f); + TestRGBColor(color, 229, 206, 212); +} + +TEST_F(TestChargingAnimation, CreateRGBAFrame) +{ + const std::size_t num_led = 20; + YAML::Node animation_description = YAML::Load("{duration: 2.0}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); + + std::array color{70, 120, 91}; + auto frame = animation_->CreateRGBAFrame(color, 1.0); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 70, 120, 91, 255); + + // reduce brightness + frame = animation_->CreateRGBAFrame(color, 0.5); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 35, 60, 45, 255); +} + +TEST_F(TestChargingAnimation, SetParam) +{ + EXPECT_THROW(animation_->SetParam("not_a_number"), std::invalid_argument); + EXPECT_NO_THROW(animation_->SetParam("0.7")); +} + +TEST_F(TestChargingAnimation, UpdateFrame) +{ + const std::size_t num_led = 20; + YAML::Node animation_description = YAML::Load("{duration: 2.0}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); + + // UpdateFrame depends on parent class variables which can be only updated using Call method. + // For full battery whole animation should be a solid green color + animation_->SetParam("1.0"); + for (std::uint8_t i = 0; i < animation_->GetAnimationLength(); i++) { + auto frame = animation_->Call(); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 0, 255, 0, 255); + } + + animation_->Reset(); + animation_->SetParam("0.5"); + std::vector frame; + // the beginning of the animation should be dark + frame = animation_->Call(); + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 0, 0, 0, 255); + + // reach middle of the animation + while (animation_->GetAnimationIteration() < animation_->GetAnimationLength() / 2) { + frame = animation_->Call(); + } + + // the middle of animation for param 0.5 should be yellow + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 255, 255, 0, 255); + + // reach end of the animation + while (!animation_->IsFinished()) { + frame = animation_->Call(); + } + + // the end of the animation should be dark + ASSERT_EQ(num_led * 4, frame.size()); + TestRGBAFrame(frame, 0, 0, 0, 255); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_image_animation.cpp b/panther_lights/test/test_image_animation.cpp new file mode 100644 index 000000000..8910be3c8 --- /dev/null +++ b/panther_lights/test/test_image_animation.cpp @@ -0,0 +1,223 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include + +namespace gil = boost::gil; + +class ImageAnimationWrapper : public panther_lights::ImageAnimation +{ +public: + ImageAnimationWrapper() {} + + std::filesystem::path ParseImagePath(const std::string & image_path) const + { + return ImageAnimation::ParseImagePath(image_path); + } + + gil::rgba8_image_t RGBAImageResize( + const gil::rgba8_image_t & image, const std::size_t width, const std::size_t height) + { + return ImageAnimation::RGBAImageResize(image, width, height); + } + + void RGBAImageConvertColor(gil::rgba8_image_t & image, const std::uint32_t color) + { + return ImageAnimation::RGBAImageConvertColor(image, color); + } + + gil::gray_alpha8_image_t RGBAImageConvertToGrey(gil::rgba8_image_t & image) + { + return ImageAnimation::RGBAImageConvertToGrey(image); + } + + void GreyImageNormalizeBrightness(gil::gray_alpha8_image_t & image) + { + return ImageAnimation::GreyImageNormalizeBrightness(image); + } + + std::vector UpdateFrame() { return ImageAnimation::UpdateFrame(); } +}; + +class TestImageAnimation : public testing::Test +{ +public: + TestImageAnimation(); + ~TestImageAnimation(); + +protected: + std::string test_image_path = "/tmp/test_image.png"; + std::unique_ptr animation_; +}; + +TestImageAnimation::TestImageAnimation() +{ + gil::rgba8_image_t image(100, 100); + gil::write_view(test_image_path, gil::const_view(image), gil::png_tag()); + animation_ = std::make_unique(); +} + +TestImageAnimation::~TestImageAnimation() { std::filesystem::remove(test_image_path); } + +TEST_F(TestImageAnimation, ParseImagePath) +{ + std::string image_path = "not/a/global/path"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + image_path = "/path/to/no/existing/file"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + // should return the same path if path is global and file exists + EXPECT_EQ(this->test_image_path, animation_->ParseImagePath(this->test_image_path)); + + // test ROS package path + image_path = "$(find invalid_ros_package)/animations/strip01_red.png"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + // invalid substitution + image_path = "$(fin panther_lights)/animations/strip01_red.png"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + image_path = "$(find panther_lights/animations/strip01_red.png"; + EXPECT_THROW(animation_->ParseImagePath(image_path), std::runtime_error); + + // following ones may not work if ROS package is not build or sourced + image_path = "$(find panther_lights)/animations/strip01_red.png"; + EXPECT_NO_THROW(animation_->ParseImagePath(image_path)); + + // multiple spaces after find syntax + image_path = "$(find panther_lights)/animations/strip01_red.png"; + EXPECT_NO_THROW(animation_->ParseImagePath(image_path)); +} + +TEST_F(TestImageAnimation, RGBAImageResize) +{ + gil::rgba8_image_t input_image(100, 100); + auto output_image = animation_->RGBAImageResize(input_image, 75, 112); + EXPECT_EQ(75, output_image.width()); + EXPECT_EQ(112, output_image.height()); +} + +TEST_F(TestImageAnimation, RGBAImageConvertColor) +{ + gil::rgba8_image_t white_image(5, 5); + gil::fill_pixels(gil::view(white_image), gil::rgba8_pixel_t(255, 255, 255, 255)); + + // copy white image + auto blue_image = white_image; + animation_->RGBAImageConvertColor(blue_image, 0x0000FF); + // expect r,g pixels to be 0, b to be 255 + gil::for_each_pixel(gil::view(blue_image), [](const gil::rgba8_pixel_t & pixel) { + EXPECT_EQ(0, pixel[0]); + EXPECT_EQ(0, pixel[1]); + EXPECT_EQ(255, pixel[2]); + EXPECT_EQ(255, pixel[3]); + }); + + // copy white image + auto green_image = white_image; + animation_->RGBAImageConvertColor(green_image, 0x00FF00); + // expect r,b pixels to be 0, g to be 255 + gil::for_each_pixel(gil::view(green_image), [](const gil::rgba8_pixel_t & pixel) { + EXPECT_EQ(0, pixel[0]); + EXPECT_EQ(255, pixel[1]); + EXPECT_EQ(0, pixel[2]); + EXPECT_EQ(255, pixel[3]); + }); + + // copy white image + auto red_image = white_image; + animation_->RGBAImageConvertColor(red_image, 0xFF0000); + // expect g,b pixels to be 0, r to be 255 + gil::for_each_pixel(gil::view(red_image), [](const gil::rgba8_pixel_t & pixel) { + EXPECT_EQ(255, pixel[0]); + EXPECT_EQ(0, pixel[1]); + EXPECT_EQ(0, pixel[2]); + EXPECT_EQ(255, pixel[3]); + }); +} + +TEST_F(TestImageAnimation, RGBAImageConvertToGrey) +{ + gil::rgba8_image_t rgb_image(5, 5); + gil::fill_pixels(gil::view(rgb_image), gil::rgba8_pixel_t(30, 100, 200, 255)); + + const int expected_grey_value = 0.299 * 30 + 0.587 * 100 + 0.114 * 200; + auto grey_image = animation_->RGBAImageConvertToGrey(rgb_image); + gil::for_each_pixel( + gil::view(grey_image), [expected_grey_value](const gil::gray_alpha8_pixel_t & pixel) { + EXPECT_EQ(expected_grey_value, pixel[0]); + }); +} + +TEST_F(TestImageAnimation, GreyImageNormalizeBrightness) +{ + gil::gray_alpha8_image_t grey_image(5, 5); + gil::fill_pixels(gil::view(grey_image), gil::gray_alpha8_pixel_t(204)); + // make the first row a bit darker + for (int i = 0; i < grey_image.width(); i++) { + gil::at_c<0>(*gil::view(grey_image).at(i)) = 102; + } + + animation_->GreyImageNormalizeBrightness(grey_image); + // check the first row + for (int i = 0; i < grey_image.width(); i++) { + EXPECT_EQ(int(102.0 / 204 * 255), gil::at_c<0>(*gil::view(grey_image).at(i))); + } + // check the rest of the image + for (int i = grey_image.width(); i < grey_image.width() * grey_image.height(); i++) { + EXPECT_EQ(255, gil::at_c<0>(*gil::view(grey_image).at(i))); + } +} + +TEST_F(TestImageAnimation, Initialize) +{ + YAML::Node animation_description = YAML::Load("{duration: 2.0}"); + + // missing image in description + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); + + animation_description["image"] = this->test_image_path; + EXPECT_NO_THROW(animation_->Initialize(animation_description, 10, 10.0)); +} + +TEST_F(TestImageAnimation, UpdateFrame) +{ + const std::size_t num_led = 20; + YAML::Node animation_description = + YAML::Load("{duration: 2.0, image: " + this->test_image_path + "}"); + + ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); + + auto frame = animation_->UpdateFrame(); + EXPECT_EQ(num_led * 4, frame.size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 3c2ad2d73690de49dcebbbc81567e36d575da112 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Tue, 20 Feb 2024 09:02:42 +0100 Subject: [PATCH 02/32] ROS 2 lights converter (#223) * add led_segment * WIP led_panel and segment converter * simplify converter * update segment conversion * add test for led panel, segment, and converter * review fixes * update copyright year * update controller so it somehow works * Update tests * Apply review fixes * fix gpio tests --- panther_gpiod/test/test_gpio_driver.cpp | 12 +- panther_lights/CMakeLists.txt | 34 +++- .../panther_lights/animation/animation.hpp | 45 ++++- .../animation/charging_animation.hpp | 2 +- .../animation/image_animation.hpp | 4 +- .../include/panther_lights/apa102.hpp | 2 +- .../panther_lights/controller_node.hpp | 2 +- .../include/panther_lights/led_panel.hpp | 54 +++++ .../include/panther_lights/led_segment.hpp | 90 +++++++++ .../panther_lights/segment_converter.hpp | 41 ++++ panther_lights/package.xml | 1 + .../src/animation/charging_animation.cpp | 2 +- .../src/animation/image_animation.cpp | 2 +- panther_lights/src/controller_node.cpp | 98 ++++++--- panther_lights/src/controller_node_main.cpp | 2 +- panther_lights/src/led_panel.cpp | 46 +++++ panther_lights/src/led_segment.cpp | 127 ++++++++++++ panther_lights/src/segment_converter.cpp | 44 ++++ panther_lights/test/test_animation.cpp | 47 +++-- .../test/test_charging_animation.cpp | 17 +- panther_lights/test/test_image_animation.cpp | 2 +- panther_lights/test/test_led_panel.cpp | 131 ++++++++++++ panther_lights/test/test_led_segment.cpp | 189 ++++++++++++++++++ .../test/test_segment_converter.cpp | 136 +++++++++++++ .../panther_utils/test/ros_test_utils.hpp | 2 +- .../include/panther_utils/test/test_utils.hpp | 27 +-- panther_utils/test/test_test_utils.cpp | 49 ++++- 27 files changed, 1107 insertions(+), 101 deletions(-) create mode 100644 panther_lights/include/panther_lights/led_panel.hpp create mode 100644 panther_lights/include/panther_lights/led_segment.hpp create mode 100644 panther_lights/include/panther_lights/segment_converter.hpp create mode 100644 panther_lights/src/led_panel.cpp create mode 100644 panther_lights/src/led_segment.cpp create mode 100644 panther_lights/src/segment_converter.cpp create mode 100644 panther_lights/test/test_led_panel.cpp create mode 100644 panther_lights/test/test_led_segment.cpp create mode 100644 panther_lights/test/test_segment_converter.cpp diff --git a/panther_gpiod/test/test_gpio_driver.cpp b/panther_gpiod/test/test_gpio_driver.cpp index f4da7a928..cd5b40487 100644 --- a/panther_gpiod/test/test_gpio_driver.cpp +++ b/panther_gpiod/test/test_gpio_driver.cpp @@ -98,9 +98,9 @@ TEST(TestGPIODriverInitialization, WrongPinConfigFail) TEST_F(TestGPIODriver, SetWrongPinValue) { - panther_utils::test_utils::ExpectThrowWithDescription( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->SetPinValue(static_cast(-1), true); }, - "Pin not found in GPIO info storage."); + "Pin not found in GPIO info storage.")); } TEST_F(TestGPIODriver, IsPinAvaible) @@ -136,12 +136,12 @@ TEST_F(TestGPIODriver, GPIOMonitorEnableUseRT) TEST_F(TestGPIODriver, GPIOEventCallbackFailWhenNoMonitorThread) { - panther_utils::test_utils::ExpectThrowWithDescription( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->ConfigureEdgeEventCallback( std::bind(&TestGPIODriver::GPIOEventCallback, this, std::placeholders::_1)); }, - "GPIO monitor thread is not running!"); + "GPIO monitor thread is not running!")); } TEST_F(TestGPIODriver, GPIOEventCallbackShareNewPinState) @@ -170,9 +170,9 @@ TEST_F(TestGPIODriver, ChangePinDirection) this->gpio_driver_->GPIOMonitorEnable(); this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::INPUT); - panther_utils::test_utils::ExpectThrowWithDescription( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { this->gpio_driver_->SetPinValue(GPIOPin::LED_SBC_SEL, true); }, - "Cannot set value for INPUT pin."); + "Cannot set value for INPUT pin.")); this->gpio_driver_->ChangePinDirection(GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT); diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 7aaac260e..bed7e5a22 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(diagnostic_updater REQUIRED) find_package(image_transport REQUIRED) find_package(panther_gpiod REQUIRED) find_package(panther_msgs REQUIRED) +find_package(panther_utils REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(sensor_msgs REQUIRED) @@ -37,8 +38,10 @@ ament_target_dependencies( rclcpp sensor_msgs) -add_executable(controller_node src/controller_node_main.cpp - src/controller_node.cpp) +add_executable( + controller_node + src/controller_node_main.cpp src/controller_node.cpp src/led_segment.cpp + src/led_panel.cpp src/segment_converter.cpp) ament_target_dependencies(controller_node rclcpp pluginlib sensor_msgs) target_link_libraries(controller_node yaml-cpp) @@ -92,6 +95,33 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_charging_animation ament_index_cpp pluginlib) target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp) + + ament_add_gtest(${PROJECT_NAME}_test_led_panel test/test_led_panel.cpp + src/led_panel.cpp) + target_include_directories( + ${PROJECT_NAME}_test_led_panel + PUBLIC $ + $) + + ament_add_gtest(${PROJECT_NAME}_test_led_segment test/test_led_segment.cpp + src/led_segment.cpp) + target_include_directories( + ${PROJECT_NAME}_test_led_segment + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_led_segment pluginlib + panther_utils) + target_link_libraries(${PROJECT_NAME}_test_led_segment yaml-cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_segment_converter test/test_segment_converter.cpp + src/segment_converter.cpp src/led_panel.cpp src/led_segment.cpp) + target_include_directories( + ${PROJECT_NAME}_test_segment_converter + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib) + target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp) endif() ament_package() diff --git a/panther_lights/include/panther_lights/animation/animation.hpp b/panther_lights/include/panther_lights/animation/animation.hpp index 6773e7dee..22863da96 100644 --- a/panther_lights/include/panther_lights/animation/animation.hpp +++ b/panther_lights/include/panther_lights/animation/animation.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,6 +15,7 @@ #ifndef PANTHER_LIGHTS_ANIMATION_HPP_ #define PANTHER_LIGHTS_ANIMATION_HPP_ +#include #include #include #include @@ -48,6 +49,7 @@ class Animation { Reset(); num_led_ = num_led; + frame_ = std::vector(num_led_ * kRGBAColorLen, 0); if (!animation_description["duration"]) { throw std::runtime_error("Missing 'duration' in animation description"); @@ -84,17 +86,16 @@ class Animation } /** - * @brief Update and return animation frame + * @brief Update animation frame * - * @returns the newest animation frame, if animation is finished will return vector filled with 0 * @exception std::runtime_error if UpdateFrame() method returns frame with invalid size */ - std::vector Call() + void Update() { if (current_cycle_ < loops_) { auto frame = UpdateFrame(); - if (frame.size() != num_led_ * 4) { + if (frame.size() != num_led_ * kRGBAColorLen) { throw std::runtime_error( "Invalid frame size. Check animation UpdateFrame() method implementation"); } @@ -111,10 +112,11 @@ class Animation finished_ = true; } - return frame; + frame_ = frame; + return; } - return std::vector(num_led_ * 4, 0); + std::fill(frame_.begin(), frame_.end(), 0); } /** @@ -129,22 +131,46 @@ class Animation progress_ = 0.0; } + /** + * @brief Return animation frame + * + * @param invert_frame_order if true will return frame with inverted RGBA values order (last 4 + * values will become first etc.) + * + * @return the newest animation frame, if animation is finished will return vector filled with 0 + */ + std::vector GetFrame(const bool invert_frame_order = false) + { + return invert_frame_order ? InvertRGBAFrame(frame_) : frame_; + } + + virtual void SetParam(const std::string & /*param*/){}; + bool IsFinished() const { return finished_; } std::size_t GetNumberOfLeds() const { return num_led_; } std::uint8_t GetBrightness() const { return brightness_; } float GetProgress() const { return progress_; } - virtual void SetParam(const std::string & /*param*/){}; + static constexpr std::size_t kRGBAColorLen = 4; protected: Animation() {} /** * @brief Abstract method that has to be implemented inside child class - * it should return RGBA animation frame with size equal to num_led_ * 4 + * it should return RGBA animation frame with size equal to num_led_ * kRGBAColorLen */ virtual std::vector UpdateFrame() = 0; + std::vector InvertRGBAFrame(const std::vector & frame) const + { + std::vector inverted_frame(frame.size()); + for (std::size_t i = 0; i < frame.size(); i += kRGBAColorLen) { + std::copy(frame.end() - i - kRGBAColorLen, frame.end() - i, inverted_frame.begin() + i); + } + return inverted_frame; + } + std::size_t GetAnimationLength() const { return anim_len_; } std::size_t GetAnimationIteration() const { return anim_iteration_; } @@ -161,6 +187,7 @@ class Animation std::uint8_t brightness_ = 255; std::string param_; + std::vector frame_; }; } // namespace panther_lights diff --git a/panther_lights/include/panther_lights/animation/charging_animation.hpp b/panther_lights/include/panther_lights/animation/charging_animation.hpp index 3a9c3f377..8b29b2651 100644 --- a/panther_lights/include/panther_lights/animation/charging_animation.hpp +++ b/panther_lights/include/panther_lights/animation/charging_animation.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_lights/include/panther_lights/animation/image_animation.hpp b/panther_lights/include/panther_lights/animation/image_animation.hpp index 5f50e4a27..008a6d03a 100644 --- a/panther_lights/include/panther_lights/animation/image_animation.hpp +++ b/panther_lights/include/panther_lights/animation/image_animation.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -52,7 +52,7 @@ class ImageAnimation : public Animation * @param image_path raw path to an image, it should be a global path or should contain '$(find * ros_package)` syntax * - * @returns global path to an image file + * @return global path to an image file * @exception std::runtime_error if provided image_path is invalid or file does not exists */ std::filesystem::path ParseImagePath(const std::string & image_path) const; diff --git a/panther_lights/include/panther_lights/apa102.hpp b/panther_lights/include/panther_lights/apa102.hpp index edbfb1e07..36a8386da 100644 --- a/panther_lights/include/panther_lights/apa102.hpp +++ b/panther_lights/include/panther_lights/apa102.hpp @@ -62,7 +62,7 @@ class APA102 * @brief Creates buffer with BGR format with structure appropriate for * the SPI transfer based on a given RGBA frame * - * @returns buffer vector. + * @return buffer vector. * * @exception std::runtime_error if frame has incorrect number of bytes */ diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 1c88b8257..48d8ed674 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_lights/include/panther_lights/led_panel.hpp b/panther_lights/include/panther_lights/led_panel.hpp new file mode 100644 index 000000000..02ab5f159 --- /dev/null +++ b/panther_lights/include/panther_lights/led_panel.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_LED_PANEL_HPP_ +#define PANTHER_LIGHTS_LED_PANEL_HPP_ + +#include +#include + +namespace panther_lights +{ + +/** + * @brief Class that represents LED panel of the robot + */ +class LEDPanel +{ +public: + LEDPanel(const std::size_t num_led); + + ~LEDPanel() = default; + + /** + * @brief Updates LED panel frame + * + * @param iterator_first position at which values will be inserted + * @param values vector with values that will be inserted into the frame + * + * @exception std::runtime_error if values vector is empty or can't be fit into the farme + */ + void UpdateFrame(const std::size_t iterator_first, const std::vector & values); + + std::vector GetFrame() const { return frame_; } + std::size_t GetNumberOfLeds() const { return num_led_; } + +private: + const std::size_t num_led_; + std::vector frame_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_LED_PANEL_HPP_ diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp new file mode 100644 index 000000000..d45d76c32 --- /dev/null +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -0,0 +1,90 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_LED_SEGMENT_HPP_ +#define PANTHER_LIGHTS_LED_SEGMENT_HPP_ + +#include +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +/** + * @brief Class that represents virtual LED segment of the robot + */ +class LEDSegment +{ +public: + /** + * @brief Parses basic parameters of the LED segment + * + * @param segment_description YAML description of the segment. Must contain given keys: + * - led_range (string) - two numbers with hyphen in between, eg.: '0-45', + * - channel (int) - id of phisical LED channel to which segment is assigned. + * @param controller_frequency frequency at which animation will be updated. + * + * @exception std::runtime_error or std::invalid_argument if missing required description key or + * key couldn't be parsed + */ + LEDSegment(const YAML::Node & segment_description, const float controller_frequency); + + ~LEDSegment(); + + /** + * @brief Overwrite current animation + * + * @param animation_description YAML description of the animation. Must contain 'type' key - + * pluginlib animation type + * + * @exception std::runtime_error if 'type' key is missing, given pluginlib fails to load or + * animation fails to initialize + */ + void SetAnimation(const YAML::Node & animation_description); + + /** + * @brief Update animation frame + * + * @exception std::runtime_error if fails to update animation + */ + void UpdateAnimation(const std::string & param = ""); + + std::size_t GetFirstLEDPosition() const; + std::size_t GetChannel() const { return channel_; } + std::vector GetAnimationFrame() const + { + return animation_->GetFrame(invert_led_order_); + } + +private: + const float controller_frequency_; + bool invert_led_order_ = false; + std::size_t channel_; + std::size_t first_led_iterator_; + std::size_t last_led_iterator_; + std::size_t num_led_; + + std::shared_ptr animation_; + std::shared_ptr> animation_loader_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_LED_SEGMENT_HPP_ diff --git a/panther_lights/include/panther_lights/segment_converter.hpp b/panther_lights/include/panther_lights/segment_converter.hpp new file mode 100644 index 000000000..502ef4b12 --- /dev/null +++ b/panther_lights/include/panther_lights/segment_converter.hpp @@ -0,0 +1,41 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_SEGMENT_CONVERTER_HPP_ +#define PANTHER_LIGHTS_SEGMENT_CONVERTER_HPP_ + +#include +#include +#include + +#include +#include + +namespace panther_lights +{ + +class SegmentConverter +{ +public: + SegmentConverter() = default; + ~SegmentConverter() = default; + + void Convert( + const std::vector> & segments, + const std::unordered_map> & panels); +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_SEGMENT_CONVERTER_HPP_ diff --git a/panther_lights/package.xml b/panther_lights/package.xml index ecac329d4..265713204 100644 --- a/panther_lights/package.xml +++ b/panther_lights/package.xml @@ -21,6 +21,7 @@ libpng-dev panther_gpiod panther_msgs + panther_utils pluginlib rclcpp sensor_msgs diff --git a/panther_lights/src/animation/charging_animation.cpp b/panther_lights/src/animation/charging_animation.cpp index 411c491bd..1e1cefc3e 100644 --- a/panther_lights/src/animation/charging_animation.cpp +++ b/panther_lights/src/animation/charging_animation.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_lights/src/animation/image_animation.cpp b/panther_lights/src/animation/image_animation.cpp index b80d34707..2fb930cf3 100644 --- a/panther_lights/src/animation/image_animation.cpp +++ b/panther_lights/src/animation/image_animation.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index b13b79c07..11683f5f7 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -21,61 +21,97 @@ #include -#include - #include +#include +#include + namespace panther_lights { ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { - animation_loader_ = std::make_shared>( - "panther_lights", "panther_lights::Animation"); + YAML::Node anim_desc = YAML::LoadFile("/home/ros/ros2_ws/src/test2.yaml"); + std::cout << "yaml loaded" << std::endl; - YAML::Node anim_desc = YAML::LoadFile("/home/ros/ros2_ws/src/test.yaml"); + std::map> segments; - try { - std::shared_ptr image_anim = - animation_loader_->createSharedInstance("panther_lights::ImageAnimation"); - int num_led = 46; - image_anim->Initialize(anim_desc, num_led, 47.0); + for (auto & segment : anim_desc["segments"].as>()) { + segments.insert( + {segment["name"].as(), std::make_shared(segment, 50.0)}); + // default anim i guess + segments[segment["name"].as()]->SetAnimation(anim_desc["animation"]); + } + std::cout << "segments created" << std::endl; + + auto segment_converter = std::make_shared(); + // std::vector::SharedPtr> panel_publishers; + std::unordered_map::SharedPtr> + panel_publishers; + std::unordered_set led_panels_channels; + + int num_led = anim_desc["num_led"].as(); + + std::unordered_map> led_panels; + + for (auto & panel : anim_desc["panels"].as>()) { + // parse params + auto channel = panel["channel"].as(); + auto number_of_leds = panel["number_of_leds"].as(); + // led_panels_channels.insert(channel); + + // create panels and publishers + led_panels.insert({channel, std::make_unique(number_of_leds)}); + panel_publishers.insert( + {channel, this->create_publisher( + "lights/driver/channel_" + std::to_string(channel) + "_frame", 10)}); + } + + // // for reconsideration - creating PanelPublisher(channel, num_led) + // for (auto & [key, pub] : panel_publishers) { + // std::unordered_map> led_panelssss = { + // pub->GetChannel(), pub->GetPanel()}; + // } - auto pub = this->create_publisher("frame", 10); + // std::vector> segments_vec = {segments["fr"], segments["fl"], + // segments["rr"], segments["rl"]}; + std::vector> segments_vec = {segments["front"], segments["rear"]}; + + std::cout << "init finished, entering loop" << std::endl; + + try { + auto frame = std::vector(num_led * 4, 0); sensor_msgs::msg::Image image; image.header.frame_id = "frame_id"; // image.header.stamp = rospy.Time.now(); image.encoding = "rgba8"; image.height = 1; + image.width = num_led; image.step = 4 * num_led; + image.data = frame; while (rclcpp::ok()) { - if (image_anim->IsFinished()) { - image_anim->Reset(); + // if (image_anim->IsFinished()) { + // image_anim->Reset(); + // } + + for (auto & segment : segments) { + segment.second->UpdateAnimation(); } - auto a = image_anim->Call(); - image.data = a; - pub->publish(image); + segment_converter->Convert(segments_vec, led_panels); + for (auto & [channel, panel] : led_panels) { + const auto frame = panel->GetFrame(); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + image.data = frame; + image.header.stamp = this->get_clock()->now(); - // for (auto & item : a) { - // std::cout << unsigned(item) << ", "; - // } - // std::cout << "]]]" << std::endl; - // for (auto & item : image.data) { - // std::cout << unsigned(item) << ", "; - // } - // std::cout << "]]]" << std::endl; - } + panel_publishers.at(channel)->publish(image); + } - auto a = image_anim->Call(); - // for (auto & item : a) { - // std::cout << unsigned(item) << ", "; - // } - // std::cout << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } } catch (pluginlib::PluginlibException & e) { printf("The plugin failed to load. Error: %s\n", e.what()); diff --git a/panther_lights/src/controller_node_main.cpp b/panther_lights/src/controller_node_main.cpp index 9f747366e..94d61ffdc 100644 --- a/panther_lights/src/controller_node_main.cpp +++ b/panther_lights/src/controller_node_main.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_lights/src/led_panel.cpp b/panther_lights/src/led_panel.cpp new file mode 100644 index 000000000..8d56362c3 --- /dev/null +++ b/panther_lights/src/led_panel.cpp @@ -0,0 +1,46 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +namespace panther_lights +{ + +LEDPanel::LEDPanel(const std::size_t num_led) : num_led_(num_led) +{ + frame_ = std::vector(num_led_ * 4, 0); +} + +void LEDPanel::UpdateFrame( + const std::size_t iterator_first, const std::vector & values) +{ + if (values.empty()) { + throw std::runtime_error("Values vector can't be empty"); + } + if (values.size() > frame_.size()) { + throw std::runtime_error("Values size is greater than frame size"); + } + if (values.size() + iterator_first > frame_.size()) { + throw std::runtime_error("Values can't be fit into the frame at given position"); + } + + std::copy(values.begin(), values.end(), frame_.begin() + iterator_first); +} + +} // namespace panther_lights diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp new file mode 100644 index 000000000..5d217205f --- /dev/null +++ b/panther_lights/src/led_segment.cpp @@ -0,0 +1,127 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include + +#include + +namespace panther_lights +{ + +LEDSegment::LEDSegment(const YAML::Node & segment_description, const float controller_frequency) +: controller_frequency_(controller_frequency) +{ + if (!segment_description["led_range"]) { + throw std::runtime_error("Missing 'led_range' in segment description"); + } + + if (!segment_description["channel"]) { + throw std::runtime_error("Missing 'channel' in segment description"); + } + + try { + channel_ = segment_description["channel"].as(); + } catch (const YAML::BadConversion & e) { + throw std::invalid_argument("Invalid channel expression: " + std::string(e.what())); + } + + const auto led_range = segment_description["led_range"].as(); + const std::size_t split_char = led_range.find('-'); + + if (split_char == std::string::npos) { + throw std::invalid_argument("No '-' character found in the led_range expression"); + } + + try { + first_led_iterator_ = std::stoi(led_range.substr(0, split_char)); + last_led_iterator_ = std::stoi(led_range.substr(split_char + 1)); + + if (first_led_iterator_ > last_led_iterator_) { + invert_led_order_ = true; + } + } catch (const std::invalid_argument & e) { + throw std::invalid_argument("Error converting string to integer"); + } + + num_led_ = std::abs(int(last_led_iterator_ - first_led_iterator_)) + 1; + + animation_loader_ = std::make_shared>( + "panther_lights", "panther_lights::Animation"); +} + +LEDSegment::~LEDSegment() +{ + // make sure that animation is destroyed before pluginlib loader + animation_.reset(); + animation_loader_.reset(); +} + +void LEDSegment::SetAnimation(const YAML::Node & animation_description) +{ + if (!animation_description["type"]) { + throw std::runtime_error("Missing 'type' in animation description"); + } + + auto type = animation_description["type"].as(); + + animation_.reset(); + + try { + animation_ = animation_loader_->createSharedInstance(type); + } catch (pluginlib::PluginlibException & e) { + throw std::runtime_error("The plugin failed to load. Error: " + std::string(e.what())); + } + + try { + animation_->Initialize(animation_description, num_led_, controller_frequency_); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); + } catch (const std::out_of_range & e) { + throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); + } +} + +void LEDSegment::UpdateAnimation(const std::string & param) +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + if (animation_->IsFinished()) { + animation_->Reset(); + } + + if (!param.empty()) { + animation_->SetParam(param); + } + + try { + animation_->Update(); + } catch (const std::runtime_error & e) { + throw std::runtime_error("Failed to update animation: " + std::string(e.what())); + } +} + +std::size_t LEDSegment::GetFirstLEDPosition() const +{ + return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen; +} + +} // namespace panther_lights diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp new file mode 100644 index 000000000..5675a1a89 --- /dev/null +++ b/panther_lights/src/segment_converter.cpp @@ -0,0 +1,44 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace panther_lights +{ + +void SegmentConverter::Convert( + const std::vector> & segments, + const std::unordered_map> & panels) +{ + for (auto & segment : segments) { + try { + auto panel = panels.at(segment->GetChannel()); + panel->UpdateFrame(segment->GetFirstLEDPosition(), segment->GetAnimationFrame()); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to convert segment animation to panel frame. Error: " + std::string(e.what())); + } + } +} + +} // namespace panther_lights diff --git a/panther_lights/test/test_animation.cpp b/panther_lights/test/test_animation.cpp index 6035f8003..331565ea7 100644 --- a/panther_lights/test/test_animation.cpp +++ b/panther_lights/test/test_animation.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -40,6 +40,11 @@ class AnimationWrapper : public panther_lights::Animation return std::vector(frame_size_, 147); } + std::vector InvertRGBAFrame(const std::vector & frame) const + { + return Animation::InvertRGBAFrame(frame); + } + std::size_t GetAnimationLength() const { return Animation::GetAnimationLength(); } std::size_t GetAnimationIteration() const { return Animation::GetAnimationIteration(); } void SetFrameSize(const std::size_t frame_size) { frame_size_ = frame_size; } @@ -119,11 +124,9 @@ TEST_F(TestAnimation, CheckInitialValues) TEST_F(TestAnimation, Reset) { ASSERT_NO_THROW(animation_->Initialize(YAML::Load("{duration: 2.0}"), 10, 10.0f)); - animation_->Call(); - EXPECT_NO_THROW(animation_->Call()); - // call animation - ASSERT_NO_THROW(animation_->Call()); + // update animation + ASSERT_NO_THROW(animation_->Update()); EXPECT_NE(std::size_t(0), animation_->GetAnimationIteration()); EXPECT_NE(0.0F, animation_->GetProgress()); @@ -134,16 +137,16 @@ TEST_F(TestAnimation, Reset) EXPECT_FALSE(animation_->IsFinished()); } -TEST_F(TestAnimation, CallWithInvalidFrameSize) +TEST_F(TestAnimation, UpdateWithInvalidFrameSize) { ASSERT_NO_THROW(animation_->Initialize(YAML::Load("{duration: 2.0}"), 10, 10.0f)); - EXPECT_NO_THROW(animation_->Call()); + EXPECT_NO_THROW(animation_->Update()); animation_->SetFrameSize(11); - EXPECT_THROW(animation_->Call(), std::runtime_error); + EXPECT_THROW(animation_->Update(), std::runtime_error); } -TEST_F(TestAnimation, Call) +TEST_F(TestAnimation, Update) { const std::size_t num_led = 10; const float controller_frequency = 10.0; @@ -153,7 +156,7 @@ TEST_F(TestAnimation, Call) // check random progress for (std::size_t i = 0; i < 5; i++) { - ASSERT_NO_THROW(animation_->Call()); + ASSERT_NO_THROW(animation_->Update()); } EXPECT_EQ(std::size_t(5), animation_->GetAnimationIteration()); EXPECT_FALSE(animation_->IsFinished()); @@ -164,7 +167,7 @@ TEST_F(TestAnimation, Call) // reach end of first loop of animaiton for (std::size_t i = 0; i < 20; i++) { - ASSERT_NO_THROW(animation_->Call()); + ASSERT_NO_THROW(animation_->Update()); } EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); EXPECT_FALSE(animation_->IsFinished()); @@ -173,20 +176,36 @@ TEST_F(TestAnimation, Call) // reach animaiton end for (std::size_t i = 0; i < 20; i++) { - ASSERT_NO_THROW(animation_->Call()); + ASSERT_NO_THROW(animation_->Update()); } EXPECT_EQ(std::size_t(0), animation_->GetAnimationIteration()); EXPECT_TRUE(animation_->IsFinished()); EXPECT_FLOAT_EQ(1.0, animation_->GetProgress()); - // after reaching animaiton end Call() method when invoked should return frame filled with 0 - auto frame = animation_->Call(); + // after reaching animaiton end Update() method when invoked should return frame filled with 0 + animation_->Update(); + auto frame = animation_->GetFrame(); EXPECT_EQ(num_led * 4, frame.size()); for (std::size_t i = 0; i < num_led * 3; i++) { EXPECT_EQ(0, frame[i]); } } +TEST_F(TestAnimation, InvertRGBAFrame) +{ + std::vector test_frame = {0, 10, 20, 255, 30, 40, 50, 255, + 60, 70, 80, 255, 100, 110, 120, 255, + 130, 140, 150, 255, 160, 170, 180, 255}; + std::vector expected_frame = {160, 170, 180, 255, 130, 140, 150, 255, + 100, 110, 120, 255, 60, 70, 80, 255, + 30, 40, 50, 255, 0, 10, 20, 255}; + + auto inverted_frame = animation_->InvertRGBAFrame(test_frame); + for (std::size_t i = 0; i < inverted_frame.size(); i++) { + EXPECT_EQ(expected_frame[i], inverted_frame[i]); + } +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/panther_lights/test/test_charging_animation.cpp b/panther_lights/test/test_charging_animation.cpp index e5f685a3e..093dfa431 100644 --- a/panther_lights/test/test_charging_animation.cpp +++ b/panther_lights/test/test_charging_animation.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -158,11 +158,12 @@ TEST_F(TestChargingAnimation, UpdateFrame) ASSERT_NO_THROW(animation_->Initialize(animation_description, num_led, 10.0)); - // UpdateFrame depends on parent class variables which can be only updated using Call method. + // UpdateFrame depends on parent class variables which can be only updated using Update method. // For full battery whole animation should be a solid green color animation_->SetParam("1.0"); for (std::uint8_t i = 0; i < animation_->GetAnimationLength(); i++) { - auto frame = animation_->Call(); + animation_->Update(); + auto frame = animation_->GetFrame(); ASSERT_EQ(num_led * 4, frame.size()); TestRGBAFrame(frame, 0, 255, 0, 255); } @@ -170,14 +171,17 @@ TEST_F(TestChargingAnimation, UpdateFrame) animation_->Reset(); animation_->SetParam("0.5"); std::vector frame; + // the beginning of the animation should be dark - frame = animation_->Call(); + animation_->Update(); + frame = animation_->GetFrame(); ASSERT_EQ(num_led * 4, frame.size()); TestRGBAFrame(frame, 0, 0, 0, 255); // reach middle of the animation while (animation_->GetAnimationIteration() < animation_->GetAnimationLength() / 2) { - frame = animation_->Call(); + animation_->Update(); + frame = animation_->GetFrame(); } // the middle of animation for param 0.5 should be yellow @@ -186,7 +190,8 @@ TEST_F(TestChargingAnimation, UpdateFrame) // reach end of the animation while (!animation_->IsFinished()) { - frame = animation_->Call(); + animation_->Update(); + frame = animation_->GetFrame(); } // the end of the animation should be dark diff --git a/panther_lights/test/test_image_animation.cpp b/panther_lights/test/test_image_animation.cpp index 8910be3c8..6936d0668 100644 --- a/panther_lights/test/test_image_animation.cpp +++ b/panther_lights/test/test_image_animation.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_lights/test/test_led_panel.cpp b/panther_lights/test/test_led_panel.cpp new file mode 100644 index 000000000..51269a1b0 --- /dev/null +++ b/panther_lights/test/test_led_panel.cpp @@ -0,0 +1,131 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include + +class TestLEDPanel : public testing::Test +{ +public: + TestLEDPanel() { led_panel_ = std::make_unique(num_led_); } + ~TestLEDPanel() {} + +protected: + void UpdateAndTestFrame( + const std::size_t iterator_first, const std::vector & test_frame); + std::unique_ptr led_panel_; + + const std::size_t num_led_ = 46; + const std::size_t frame_size_ = num_led_ * 4; +}; + +void TestLEDPanel::UpdateAndTestFrame( + const std::size_t iterator_first, const std::vector & test_frame) +{ + // reset frame + led_panel_->UpdateFrame(0, std::vector(num_led_ * 4, 0)); + + // update frame + led_panel_->UpdateFrame(iterator_first, test_frame); + auto frame = led_panel_->GetFrame(); + + for (std::size_t i = 0; i < frame.size(); i++) { + if (i < iterator_first) { + EXPECT_EQ(frame.at(i), 0); + } else if (i < iterator_first + test_frame.size()) { + EXPECT_EQ(frame.at(i), test_frame.at(i - iterator_first)); + } else { + EXPECT_EQ(frame.at(i), 0); + } + } +} + +TEST(TestLEDPanelInitialization, FrameSize) +{ + const std::size_t num_led = 22; + auto led_panel = panther_lights::LEDPanel(num_led); + EXPECT_EQ(num_led * 4, led_panel.GetFrame().size()); +} + +TEST_F(TestLEDPanel, UpdateFrameFullFrame) +{ + const auto test_frame = std::vector(num_led_ * 4, 64); + UpdateAndTestFrame(0, test_frame); +} + +TEST_F(TestLEDPanel, UpdateFrameShortFrame) +{ + const std::size_t test_frame_size = std::size_t(frame_size_ / 2); + const auto test_frame = std::vector(test_frame_size, 77); + UpdateAndTestFrame(0, test_frame); + + const std::size_t fit_frame_in_the_middle_index = frame_size_ / 4; + UpdateAndTestFrame(fit_frame_in_the_middle_index, test_frame); + + const std::size_t fit_frame_at_the_end_index = frame_size_ - test_frame_size; + UpdateAndTestFrame(fit_frame_at_the_end_index, test_frame); +} + +TEST_F(TestLEDPanel, UpdateFrameOneElementFrame) +{ + auto frame_with_one_element = std::vector(1, 88); + UpdateAndTestFrame(frame_size_ - 1, frame_with_one_element); +} + +TEST_F(TestLEDPanel, UpdateFrameEmptyFrame) +{ + std::vector test_frame; + EXPECT_THROW(led_panel_->UpdateFrame(0, test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameInvalidValuesSize) +{ + const auto test_frame = std::vector(num_led_ * 4 + 1, 0); + EXPECT_THROW(led_panel_->UpdateFrame(0, test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameValuesOutOfRange) +{ + const auto test_frame = std::vector(num_led_ * 4, 0); + EXPECT_THROW(led_panel_->UpdateFrame(7, test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameLastFrameValueOutOfRange) +{ + const auto test_frame_size = 10; + const auto test_frame = std::vector(test_frame_size, 0); + EXPECT_THROW( + led_panel_->UpdateFrame(frame_size_ - (test_frame_size - 1), test_frame), std::runtime_error); +} + +TEST_F(TestLEDPanel, UpdateFrameIteratorOutOfRange) +{ + const auto test_frame = std::vector(10, 0); + EXPECT_THROW(led_panel_->UpdateFrame(frame_size_, test_frame), std::runtime_error); + + const auto frame_with_one_element = std::vector(1, 0); + EXPECT_THROW(led_panel_->UpdateFrame(frame_size_, frame_with_one_element), std::runtime_error); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_led_segment.cpp b/panther_lights/test/test_led_segment.cpp new file mode 100644 index 000000000..561dfb012 --- /dev/null +++ b/panther_lights/test/test_led_segment.cpp @@ -0,0 +1,189 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +class TestLEDSegment : public testing::Test +{ +public: + TestLEDSegment(); + ~TestLEDSegment() {} + +protected: + std::shared_ptr led_segment_; + + const float controller_freq = 50.0; + const std::size_t segment_led_num_ = 10; +}; + +TestLEDSegment::TestLEDSegment() +{ + const auto segment_desc = + YAML::Load("{led_range: 0-" + std::to_string(segment_led_num_ - 1) + ", channel: 1}"); + led_segment_ = std::make_shared(segment_desc, controller_freq); +} + +YAML::Node CreateSegmentDescription(const std::string & led_range, const std::string & channel) +{ + return YAML::Load("{led_range: " + led_range + ", channel: " + channel + "}"); +} + +TEST(TestLEDSegmentInitialization, DescriptionMissingRequiredKey) +{ + auto segment_desc = YAML::Load(""); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Missing 'led_range' in segment description")); + + segment_desc = YAML::Load("led_range: 0-10"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Missing 'channel' in segment description")); +} + +TEST(TestLEDSegmentInitialization, InvalidChannelExpression) +{ + auto segment_desc = CreateSegmentDescription("0-10", "s1"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Invalid channel expression: ")); + + segment_desc["channel"] = "-1"; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Invalid channel expression: ")); +} + +TEST(TestLEDSegmentInitialization, InvalidLedRangeExpression) +{ + auto segment_desc = CreateSegmentDescription("010", "1"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "No '-' character found in the led_range expression")); + + segment_desc["led_range"] = "s0-10"; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Error converting string to integer")); + + segment_desc["led_range"] = "0-p10"; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, + "Error converting string to integer")); +} + +TEST(TestLEDSegmentInitialization, ValidDescription) +{ + const auto segment_desc = CreateSegmentDescription("0-10", "1"); + EXPECT_NO_THROW(panther_lights::LEDSegment(segment_desc, 10.0)); +} + +TEST(TestLEDSegmentInitialization, FirstLedPosition) +{ + auto segment_desc = CreateSegmentDescription("0-10", "1"); + std::shared_ptr led_segment; + + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "5-11"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "10-10"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(10 * 4), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "13-5"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(5 * 4), led_segment->GetFirstLEDPosition()); + + segment_desc["led_range"] = "17-0"; + led_segment.reset(); + ASSERT_NO_THROW(led_segment = std::make_shared(segment_desc, 10.0)); + EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); +} + +TEST_F(TestLEDSegment, SetAnimationMissingTypeKey) +{ + const auto animation_desc = YAML::Load(""); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->SetAnimation(animation_desc); }, + "Missing 'type' in animation description")); +} + +TEST_F(TestLEDSegment, SetAnimationInvalidType) +{ + const auto animation_desc = YAML::Load("{type: panther_lights::WrongAnimationType}"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->SetAnimation(animation_desc); }, "The plugin failed to load. Error: ")); +} + +TEST_F(TestLEDSegment, SetAnimationFailAnimationInitialization) +{ + const auto animation_desc = YAML::Load("{type: panther_lights::ImageAnimation}"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->SetAnimation(animation_desc); }, "Failed to initialize animation: ")); +} + +TEST_F(TestLEDSegment, SetAnimation) +{ + // test each known animtion type + const auto image_anim_desc = YAML::Load( + "{type: panther_lights::ImageAnimation, " + "image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + EXPECT_NO_THROW(led_segment_->SetAnimation(image_anim_desc)); + + const auto charging_anim_desc = YAML::Load( + "{type: panther_lights::ChargingAnimation, " + "duration: 2}"); + EXPECT_NO_THROW(led_segment_->SetAnimation(charging_anim_desc)); +} + +TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->UpdateAnimation(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, UpdateAnimation) +{ + const auto anim_desc = YAML::Load( + "{type: panther_lights::ImageAnimation, " + "image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + ASSERT_NO_THROW(led_segment_->SetAnimation(anim_desc)); + EXPECT_NO_THROW(led_segment_->UpdateAnimation()); + EXPECT_EQ(segment_led_num_ * 4, led_segment_->GetAnimationFrame().size()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_lights/test/test_segment_converter.cpp b/panther_lights/test/test_segment_converter.cpp new file mode 100644 index 000000000..8636f467b --- /dev/null +++ b/panther_lights/test/test_segment_converter.cpp @@ -0,0 +1,136 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +class TestSegmentConverter : public testing::Test +{ +public: + TestSegmentConverter() + { + segment_converter_ = std::make_unique(); + + // create 2 basic panels with different number of leds + led_panels_.insert({1, std::make_unique(panel_1_num_led_)}); + led_panels_.insert({2, std::make_unique(panel_2_num_led_)}); + } + ~TestSegmentConverter() {} + +protected: + YAML::Node CreateSegmentDescription( + const std::size_t first_led, const std::size_t last_led, const std::size_t channel) const; + YAML::Node CreateImageAnimationDescription(); + + std::size_t panel_1_num_led_ = 20; + std::size_t panel_2_num_led_ = 30; + + std::unique_ptr segment_converter_; + std::vector> segments_; + std::unordered_map> led_panels_; +}; + +YAML::Node TestSegmentConverter::CreateSegmentDescription( + const std::size_t first_led, const std::size_t last_led, const std::size_t channel) const +{ + YAML::Node desc; + desc["led_range"] = std::to_string(first_led) + "-" + std::to_string(last_led); + desc["channel"] = channel; + return desc; +} + +YAML::Node TestSegmentConverter::CreateImageAnimationDescription() +{ + return YAML::Load( + "{type: panther_lights::ImageAnimation, " + "image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); +} + +TEST_F(TestSegmentConverter, ConvertInvalidChannel) +{ + segments_.push_back( + std::make_shared(CreateSegmentDescription(0, 10, 123), 50.0)); + const auto anim_desc = CreateImageAnimationDescription(); + ASSERT_NO_THROW(segments_.at(0)->SetAnimation(anim_desc)); + + EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::out_of_range); +} + +TEST_F(TestSegmentConverter, ConvertInvalidLedRange) +{ + segments_.push_back(std::make_shared( + CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 10, 1), 50.0)); + const auto anim_desc = CreateImageAnimationDescription(); + ASSERT_NO_THROW(segments_.at(0)->SetAnimation(anim_desc)); + + EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::runtime_error); +} + +TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel) +{ + segments_.push_back(std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); + segments_.push_back(std::make_shared( + CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); + + const auto anim_desc = CreateImageAnimationDescription(); + + for (auto & segment : segments_) { + ASSERT_NO_THROW(segment->SetAnimation(anim_desc)); + ASSERT_NO_THROW(segment->UpdateAnimation()); + } + + EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); +} + +TEST_F(TestSegmentConverter, ConvertMultipleSegments) +{ + segments_.push_back(std::make_shared( + CreateSegmentDescription(0, std::size_t(panel_1_num_led_ / 2) - 1, 1), 50.0)); + segments_.push_back(std::make_shared( + CreateSegmentDescription(std::size_t(panel_1_num_led_ / 2), panel_1_num_led_ - 1, 1), 50.0)); + + segments_.push_back(std::make_shared( + CreateSegmentDescription(0, (panel_2_num_led_ / 4) - 1, 2), 50.0)); + segments_.push_back(std::make_shared( + CreateSegmentDescription((panel_2_num_led_ / 4), (panel_2_num_led_ / 2) - 1, 2), 50.0)); + segments_.push_back(std::make_shared( + CreateSegmentDescription((panel_2_num_led_ / 2), panel_2_num_led_ - 1, 2), 50.0)); + + const auto anim_desc = CreateImageAnimationDescription(); + for (auto & segment : segments_) { + ASSERT_NO_THROW(segment->SetAnimation(anim_desc)); + ASSERT_NO_THROW(segment->UpdateAnimation()); + } + + EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/panther_utils/include/panther_utils/test/ros_test_utils.hpp b/panther_utils/include/panther_utils/test/ros_test_utils.hpp index 2e4ffd7b3..868bd1ddc 100644 --- a/panther_utils/include/panther_utils/test/ros_test_utils.hpp +++ b/panther_utils/include/panther_utils/test/ros_test_utils.hpp @@ -38,7 +38,7 @@ namespace panther_utils::test_utils template bool WaitForMsg( const std::shared_ptr & node, std::shared_ptr & msg, - const std::chrono::nanoseconds & timeout) + const std::chrono::milliseconds & timeout) { msg = nullptr; rclcpp::Time start_time = node->now(); diff --git a/panther_utils/include/panther_utils/test/test_utils.hpp b/panther_utils/include/panther_utils/test/test_utils.hpp index 14bf8db59..7674735b9 100644 --- a/panther_utils/include/panther_utils/test/test_utils.hpp +++ b/panther_utils/include/panther_utils/test/test_utils.hpp @@ -45,25 +45,28 @@ bool CheckNaNVector(const std::vector & vector) } /** - * @brief Tests if a method throws an exception of a given type and the error message contains the + * @brief Check if a method throws an exception of a given type and the error message contains the * provided message * * @param func The method that will be tested * @param error_msg The error message that has to be contained within the thrown message + * + * @return True if method throws an exception of a given type and the error message contains the + * provided message, false otherwise */ template -void ExpectThrowWithDescription(const Func & func, const std::string & error_msg) +bool IsMessageThrown(const Func & func, const std::string & error_msg) { - EXPECT_THROW( - { - try { - func(); - } catch (const ExceptionType & e) { - EXPECT_TRUE(std::string(e.what()).find(error_msg) != std::string::npos); - throw; - } - }, - ExceptionType); + try { + func(); + } catch (const ExceptionType & e) { + if (std::string(e.what()).find(error_msg) != std::string::npos) { + return true; + } + } catch (...) { + } + + return false; } } // namespace panther_utils::test_utils diff --git a/panther_utils/test/test_test_utils.cpp b/panther_utils/test/test_test_utils.cpp index 6346e071a..b8b0a7a58 100644 --- a/panther_utils/test/test_test_utils.cpp +++ b/panther_utils/test/test_test_utils.cpp @@ -37,22 +37,49 @@ TEST(TestTestUtils, CheckNanVector) EXPECT_THROW(TestCheckNaNVector(), std::runtime_error); } -TEST(TestTestUtils, ExpectThrowWithDescription) +TEST(TestTestUtils, IsMessageThrownTrue) { - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::runtime_error("Example exception"); }, "Example exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example exception")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::out_of_range("Example exception"); }, "Example exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::out_of_range("Example exception"); }, "Example exception")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::invalid_argument("Example exception"); }, "Example exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::invalid_argument("Example exception"); }, "Example exception")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::runtime_error("Example exception"); }, "Example"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example")); - panther_utils::test_utils::ExpectThrowWithDescription( - []() { throw std::runtime_error("Example exception"); }, "exception"); + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "exception")); +} + +TEST(TestTestUtils, IsMessageThrownDifferentException) +{ + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::out_of_range("Example exception"); }, "Example exception")); + + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::invalid_argument("Example exception"); }, "Example exception")); + + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example exception")); +} + +TEST(TestTestUtils, IsMessageThrownDifferentMessage) +{ + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Different exception message")); + + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { throw std::runtime_error("Example exception"); }, "Example exception ")); +} + +TEST(TestTestUtils, IsMessageThrownNoThrow) +{ + EXPECT_FALSE(panther_utils::test_utils::IsMessageThrown( + []() { return; }, "Example exception")); } int main(int argc, char ** argv) From d01009263bde20058ceb138c6a6babf7a1eef085 Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 20 Feb 2024 08:59:58 +0000 Subject: [PATCH 03/32] parse controller configuration --- panther_lights/CMakeLists.txt | 5 +- .../panther_lights/controller_node.hpp | 103 +++++- .../panther_lights/segment_converter.hpp | 2 +- panther_lights/src/controller_node.cpp | 305 ++++++++++++++---- panther_lights/src/led_segment.cpp | 9 +- panther_lights/src/segment_converter.cpp | 7 +- 6 files changed, 355 insertions(+), 76 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index bed7e5a22..79fbe785f 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -43,7 +43,8 @@ add_executable( src/controller_node_main.cpp src/controller_node.cpp src/led_segment.cpp src/led_panel.cpp src/segment_converter.cpp) -ament_target_dependencies(controller_node rclcpp pluginlib sensor_msgs) +ament_target_dependencies(controller_node rclcpp pluginlib panther_msgs + sensor_msgs) target_link_libraries(controller_node yaml-cpp) add_executable(dummy_scheduler_node src/dummy_scheduler_node_main.cpp @@ -57,7 +58,7 @@ install(TARGETS driver_node controller_node dummy_scheduler_node install(TARGETS panther_animation_plugins LIBRARY DESTINATION lib) -install(DIRECTORY animations launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY animations config launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include) ament_export_include_directories(include) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 48d8ed674..d73b3f0d6 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -17,16 +17,34 @@ #include #include +#include + +#include #include -#include +#include + +#include #include +#include namespace panther_lights { +using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; + +struct AnimationWrapper +{ + std::size_t id; + std::size_t priority; + std::string name; + std::vector segments; + std::vector animations; + bool repeating = false; +}; + class ControllerNode : public rclcpp::Node { public: @@ -35,7 +53,88 @@ class ControllerNode : public rclcpp::Node ~ControllerNode() {} private: - std::shared_ptr> animation_loader_; + // might be moved to separate file so animations can ues it too, or even to panther_utils + template + T GetYAMLKeyValue(const YAML::Node & description, const std::string & key) const; + + template + T GetYAMLKeyValue( + const YAML::Node & description, const std::string & key, const T default_value) const; + + void DeclareParameters(); + void LoadParameters(); + + /** + * @brief Initializes LED panel based on YAML description. This LED panel is a representation of + * the real panel of the robot + * + * @param panels_description YAML description of the panel, must contain 'channel' and + * 'number_of_leds' keys + * + * @exception std::runtime_error if initialization of the LED panel fails + */ + void InitializeLEDPanels(const YAML::Node & panels_description); + + /** + * @brief Initializes LED segment based on YAML description. This LED segment is a representation + * of the abstract segment located on the panel of the robot + * + * @param segments_description YAML description of the segment, must contain 'name', 'channel' and + * 'led_range' keys + * @param controller_freq Frequency at which animations will be processed + * + * @exception std::runtime_error if initialization of the LED segment fails + */ + void InitializeLEDSegments(const YAML::Node & segments_description, const float controller_freq); + + /** + * @brief Initializes LED segments map based on YAML description. This assigns list with segments + * names to abstract names that can be used with animations to specify on which segments animation + * should be displayed + * + * @param segments_map_description YAML description of the segments map + */ + void InitializeLEDSegmentsMap(const YAML::Node & segments_map_description); + + /** + * @brief Adds animations from YAML description to an unordered map with animations + * + * @param animations_description YAML description with list of animations to be loaded + * + * @exception std::runtime_error if fails to load an animation or animation with given ID already + * exists in the map + */ + void LoadAnimations(const YAML::Node & animations_description); + + /** + * @brief Sets animation with requested ID + * + * @param animation_id Animation ID + * + * @exception std::runtime_error animation with give ID does not exists or (to be updated) + */ + void SetAnimation(const std::size_t animation_id); + + void PublishPanelFrame(const std::size_t channel); + void SetLEDAnimationCB( + const SetLEDAnimationSrv::Request::SharedPtr & request, + SetLEDAnimationSrv::Response::SharedPtr response); + void ControllerTimerCB(); + + static constexpr std::uint8_t kDefaultAnimaitonPriority = 3; + + std::unordered_map> led_panels_; + std::unordered_map::SharedPtr> + panel_publishers_; + std::unordered_map> segments_; + std::unordered_map> segments_map_; + std::unordered_map animations_; + std::shared_ptr segment_converter_; + + rclcpp::Service::SharedPtr set_led_animation_server_; + rclcpp::TimerBase::SharedPtr controller_timer_; + + std::shared_ptr current_animation_; }; } // namespace panther_lights diff --git a/panther_lights/include/panther_lights/segment_converter.hpp b/panther_lights/include/panther_lights/segment_converter.hpp index 502ef4b12..214bc1d32 100644 --- a/panther_lights/include/panther_lights/segment_converter.hpp +++ b/panther_lights/include/panther_lights/segment_converter.hpp @@ -32,7 +32,7 @@ class SegmentConverter ~SegmentConverter() = default; void Convert( - const std::vector> & segments, + const std::unordered_map> & segments, const std::unordered_map> & panels); }; diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 11683f5f7..6779f1917 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -14,8 +14,14 @@ #include +#include +#include +#include #include +#include #include +#include +#include #include @@ -23,101 +29,270 @@ #include +#include + +#include #include #include namespace panther_lights { +using std::placeholders::_1; +using std::placeholders::_2; + ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { - YAML::Node anim_desc = YAML::LoadFile("/home/ros/ros2_ws/src/test2.yaml"); - std::cout << "yaml loaded" << std::endl; + this->declare_parameter( + "led_config_file", + "/home/husarion/ros2_ws/src/panther_ros/panther_lights/config/led_config.yaml"); + this->declare_parameter("controller_freq", 50.0); - std::map> segments; + const auto led_config_file = this->get_parameter("led_config_file").as_string(); + const float controller_freq = this->get_parameter("controller_freq").as_double(); - for (auto & segment : anim_desc["segments"].as>()) { - segments.insert( - {segment["name"].as(), std::make_shared(segment, 50.0)}); - // default anim i guess - segments[segment["name"].as()]->SetAnimation(anim_desc["animation"]); - } - std::cout << "segments created" << std::endl; + YAML::Node led_config_desc = YAML::LoadFile(led_config_file); - auto segment_converter = std::make_shared(); - // std::vector::SharedPtr> panel_publishers; - std::unordered_map::SharedPtr> - panel_publishers; - std::unordered_set led_panels_channels; + InitializeLEDPanels(led_config_desc["panels"]); + InitializeLEDSegments(led_config_desc["segments"], controller_freq); + InitializeLEDSegmentsMap(led_config_desc["segments_map"]); + LoadAnimations(led_config_desc["animations"]); - int num_led = anim_desc["num_led"].as(); + segment_converter_ = std::make_shared(); - std::unordered_map> led_panels; + set_led_animation_server_ = this->create_service( + "lights/controller/set/animation", std::bind(&ControllerNode::SetLEDAnimationCB, this, _1, _2)); - for (auto & panel : anim_desc["panels"].as>()) { - // parse params - auto channel = panel["channel"].as(); - auto number_of_leds = panel["number_of_leds"].as(); - // led_panels_channels.insert(channel); + controller_timer_ = this->create_wall_timer( + std::chrono::microseconds(static_cast(1e6 / controller_freq)), + std::bind(&ControllerNode::ControllerTimerCB, this)); - // create panels and publishers - led_panels.insert({channel, std::make_unique(number_of_leds)}); - panel_publishers.insert( - {channel, this->create_publisher( - "lights/driver/channel_" + std::to_string(channel) + "_frame", 10)}); - } + RCLCPP_INFO(this->get_logger(), "Node started"); - // // for reconsideration - creating PanelPublisher(channel, num_led) - // for (auto & [key, pub] : panel_publishers) { - // std::unordered_map> led_panelssss = { - // pub->GetChannel(), pub->GetPanel()}; - // } + // animations are initialized when they are set. No way do check if they are correctly defined + // before? +} - // std::vector> segments_vec = {segments["fr"], segments["fl"], - // segments["rr"], segments["rl"]}; - std::vector> segments_vec = {segments["front"], segments["rear"]}; +template +T ControllerNode::GetYAMLKeyValue(const YAML::Node & description, const std::string & key) const +{ + if (!description[key]) { + throw std::runtime_error("Missing '" + static_cast(key) + "' in description"); + } - std::cout << "init finished, entering loop" << std::endl; + try { + return description[key].as(); + } catch (const YAML::BadConversion & e) { + throw std::runtime_error("Failed to convert '" + static_cast(key) + "' key"); + } +} +template +T ControllerNode::GetYAMLKeyValue( + const YAML::Node & description, const std::string & key, const T default_value) const +{ try { - auto frame = std::vector(num_led * 4, 0); - sensor_msgs::msg::Image image; - image.header.frame_id = "frame_id"; - // image.header.stamp = rospy.Time.now(); - image.encoding = "rgba8"; - image.height = 1; - - image.width = num_led; - image.step = 4 * num_led; - image.data = frame; - - while (rclcpp::ok()) { - // if (image_anim->IsFinished()) { - // image_anim->Reset(); - // } - - for (auto & segment : segments) { - segment.second->UpdateAnimation(); + return GetYAMLKeyValue(description, key); + } catch (const std::runtime_error & e) { + if ( + std::string(e.what()).find( + "Missing '" + static_cast(key) + "' in description") != std::string::npos) { + return default_value; + } + throw; + } +} + +void ControllerNode::DeclareParameters() +{ + // this->declare_parameter("led_config_file"); +} + +void ControllerNode::LoadParameters() +{ + // led_config_file_ = this->get_parameter("led_config_file").as_string(); +} + +void ControllerNode::InitializeLEDPanels(const YAML::Node & panels_description) +{ + RCLCPP_INFO(this->get_logger(), "Initializing LED panels"); + + for (auto & panel : panels_description.as>()) { + const auto channel = GetYAMLKeyValue(panel, "channel"); + const auto number_of_leds = GetYAMLKeyValue(panel, "number_of_leds"); + + const auto result = led_panels_.emplace(channel, std::make_unique(number_of_leds)); + if (!result.second) { + throw std::runtime_error( + "Multiple panels with channel nr '" + std::to_string(channel) + "' found"); + } + + const auto pub_result = panel_publishers_.emplace( + channel, this->create_publisher( + "lights/driver/channel_" + std::to_string(channel) + "_frame", 10)); + if (!pub_result.second) { + throw std::runtime_error( + "Multiple panel publishers for channel nr '" + std::to_string(channel) + "' found"); + } + + RCLCPP_INFO(this->get_logger(), "Successfully initialized panel with channel nr %li", channel); + } +} + +void ControllerNode::InitializeLEDSegments( + const YAML::Node & segments_description, const float controller_freq) +{ + RCLCPP_INFO(this->get_logger(), "Initializing LED segments"); + for (auto & segment : segments_description.as>()) { + const auto segment_name = GetYAMLKeyValue(segment, "name"); + + try { + const auto result = segments_.emplace( + segment_name, std::make_shared(segment, controller_freq)); + if (!result.second) { + throw std::runtime_error("Multiple segments with given name found"); } - segment_converter->Convert(segments_vec, led_panels); - for (auto & [channel, panel] : led_panels) { - const auto frame = panel->GetFrame(); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to initialize '" + segment_name + "' segment: " + std::string(e.what())); + } catch (const std::invalid_argument & e) { + throw std::runtime_error( + "Failed to initialize '" + segment_name + "' segment: " + std::string(e.what())); + } + + RCLCPP_INFO(this->get_logger(), "Successfully initialized '%s' segment", segment_name.c_str()); + } +} + +void ControllerNode::InitializeLEDSegmentsMap(const YAML::Node & segments_map_description) +{ + for (const auto & key : segments_map_description) { + const auto name = key.first.as(); + const auto value = key.second.as>(); + segments_map_.emplace(name, value); + } +} + +void ControllerNode::LoadAnimations(const YAML::Node & animations_description) +{ + RCLCPP_INFO(this->get_logger(), "Loading LED animations"); + for (auto & animation_description : animations_description.as>()) { + AnimationWrapper animation; + + animation.id = GetYAMLKeyValue(animation_description, "id"); + animation.name = GetYAMLKeyValue(animation_description, "name"); + animation.priority = GetYAMLKeyValue( + animation_description, "priority", kDefaultAnimaitonPriority); + animation.animations = GetYAMLKeyValue>( + animation_description, "animations"); + + const auto result = animations_.emplace(animation.id, animation); + if (!result.second) { + throw std::runtime_error( + "Failed to load '" + animation.name + + "' animation: Animation with given ID already exists."); + } + } + + RCLCPP_INFO(this->get_logger(), "Animations Successfully loaded"); +} - image.data = frame; - image.header.stamp = this->get_clock()->now(); +void ControllerNode::SetAnimation(const std::size_t animation_id) +{ + if (animations_.find(animation_id) == animations_.end()) { + throw std::runtime_error("No animation with ID: " + std::to_string(animation_id)); + } + + const auto animation = animations_.at(animation_id); + + for (auto & segment_animation : animation.animations) { + auto segments_group = GetYAMLKeyValue(segment_animation, "segments"); + + std::vector segments; + try { + segments = segments_map_.at(segments_group); + } catch (const std::out_of_range & e) { + throw std::out_of_range("No segment group with name: " + segments_group); + } + + for (auto & segment : segments) { + if (segments_.find(segment) == segments_.end()) { + throw std::runtime_error("No segment with name: " + segment); + } - panel_publishers.at(channel)->publish(image); + try { + segments_.at(segment)->SetAnimation(segment_animation); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to set '" + animation.name + "' animation: " + std::string(e.what())); } + } + } - std::this_thread::sleep_for(std::chrono::milliseconds(20)); + current_animation_ = std::make_shared(animation); +} + +void ControllerNode::SetLEDAnimationCB( + const SetLEDAnimationSrv::Request::SharedPtr & request, + SetLEDAnimationSrv::Response::SharedPtr response) +{ + try { + SetAnimation(request->animation.id); + response->success = true; + } catch (const std::exception & e) { + response->success = false; + response->message = e.what(); + } +} + +void ControllerNode::PublishPanelFrame(const std::size_t channel) +{ + auto panel = led_panels_.at(channel); + const auto number_of_leds = panel->GetNumberOfLeds(); + + sensor_msgs::msg::Image image; + image.header.frame_id = "lights_channel_" + std::to_string(channel); + image.header.stamp = this->get_clock()->now(); + image.encoding = "rgba8"; + image.height = 1; + image.width = number_of_leds; + image.step = number_of_leds * 4; + image.data = panel->GetFrame(); + + panel_publishers_.at(channel)->publish(image); +} + +void ControllerNode::ControllerTimerCB() +{ + if (!current_animation_) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for animation"); + return; + } + + std::vector> segments_vec; + + for (auto & [segment_name, segment] : segments_) { + try { + segment->UpdateAnimation(); + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + this->get_logger(), "Failed to update animation on %s segment: %s", segment_name.c_str(), + e.what()); + return; } + } - } catch (pluginlib::PluginlibException & e) { - printf("The plugin failed to load. Error: %s\n", e.what()); + try { + segment_converter_->Convert(segments_, led_panels_); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(this->get_logger(), e.what()); + return; } - RCLCPP_INFO(this->get_logger(), "Node started"); + for (auto & panel : led_panels_) { + PublishPanelFrame(panel.first); + } } } // namespace panther_lights diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index 5d217205f..887c6b425 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -81,21 +81,24 @@ void LEDSegment::SetAnimation(const YAML::Node & animation_description) auto type = animation_description["type"].as(); - animation_.reset(); + std::shared_ptr animation; try { - animation_ = animation_loader_->createSharedInstance(type); + animation = animation_loader_->createSharedInstance(type); } catch (pluginlib::PluginlibException & e) { throw std::runtime_error("The plugin failed to load. Error: " + std::string(e.what())); } try { - animation_->Initialize(animation_description, num_led_, controller_frequency_); + animation->Initialize(animation_description, num_led_, controller_frequency_); } catch (const std::runtime_error & e) { throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); } catch (const std::out_of_range & e) { throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); } + + animation_.reset(); + animation_ = std::move(animation); } void LEDSegment::UpdateAnimation(const std::string & param) diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp index 5675a1a89..ea16f346b 100644 --- a/panther_lights/src/segment_converter.cpp +++ b/panther_lights/src/segment_converter.cpp @@ -27,16 +27,17 @@ namespace panther_lights { void SegmentConverter::Convert( - const std::vector> & segments, + const std::unordered_map> & segments, const std::unordered_map> & panels) { - for (auto & segment : segments) { + for (auto & [segment_name, segment] : segments) { try { auto panel = panels.at(segment->GetChannel()); panel->UpdateFrame(segment->GetFirstLEDPosition(), segment->GetAnimationFrame()); } catch (const std::runtime_error & e) { throw std::runtime_error( - "Failed to convert segment animation to panel frame. Error: " + std::string(e.what())); + "Failed to convert '" + segment_name + + "' segment animation to panel frame. Error: " + std::string(e.what())); } } } From b5b13eb730e2788ff30d7d6b8dfe28edb1e188ac Mon Sep 17 00:00:00 2001 From: Dawid Date: Wed, 21 Feb 2024 08:59:59 +0000 Subject: [PATCH 04/32] add default animation --- .../include/panther_lights/led_segment.hpp | 18 +++++--- panther_lights/src/led_segment.cpp | 41 +++++++++++++++---- 2 files changed, 45 insertions(+), 14 deletions(-) diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index d45d76c32..696d91bb4 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -53,11 +53,12 @@ class LEDSegment * * @param animation_description YAML description of the animation. Must contain 'type' key - * pluginlib animation type + * @param repeating if true, will set the default animation for the panel * * @exception std::runtime_error if 'type' key is missing, given pluginlib fails to load or * animation fails to initialize */ - void SetAnimation(const YAML::Node & animation_description); + void SetAnimation(const YAML::Node & animation_description, const bool repeating); /** * @brief Update animation frame @@ -66,12 +67,18 @@ class LEDSegment */ void UpdateAnimation(const std::string & param = ""); + /** + * @brief Get current animation frame + * + * @return Current animation frame or default animation frame if it was defined and the main + * animation is finished + * @exception std::runtime_error if segment animation is not defined + */ + std::vector GetAnimationFrame() const; + std::size_t GetFirstLEDPosition() const; + std::size_t GetChannel() const { return channel_; } - std::vector GetAnimationFrame() const - { - return animation_->GetFrame(invert_led_order_); - } private: const float controller_frequency_; @@ -82,6 +89,7 @@ class LEDSegment std::size_t num_led_; std::shared_ptr animation_; + std::shared_ptr default_animation_; std::shared_ptr> animation_loader_; }; diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index 887c6b425..50d44be9e 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -68,12 +68,13 @@ LEDSegment::LEDSegment(const YAML::Node & segment_description, const float contr LEDSegment::~LEDSegment() { - // make sure that animation is destroyed before pluginlib loader + // make sure that animations are destroyed before pluginlib loader animation_.reset(); + default_animation_.reset(); animation_loader_.reset(); } -void LEDSegment::SetAnimation(const YAML::Node & animation_description) +void LEDSegment::SetAnimation(const YAML::Node & animation_description, const bool repeating) { if (!animation_description["type"]) { throw std::runtime_error("Missing 'type' in animation description"); @@ -97,8 +98,13 @@ void LEDSegment::SetAnimation(const YAML::Node & animation_description) throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); } - animation_.reset(); animation_ = std::move(animation); + + if (repeating) { + default_animation_ = animation_; + } else if (default_animation_) { + default_animation_->Reset(); + } } void LEDSegment::UpdateAnimation(const std::string & param) @@ -107,21 +113,38 @@ void LEDSegment::UpdateAnimation(const std::string & param) throw std::runtime_error("Segment animation not defined"); } - if (animation_->IsFinished()) { - animation_->Reset(); - } + std::shared_ptr animation; - if (!param.empty()) { - animation_->SetParam(param); + if (animation_->IsFinished() && default_animation_) { + animation = default_animation_; + if (animation->IsFinished()) { + animation->Reset(); + } + } else { + animation = animation_; } try { - animation_->Update(); + animation->SetParam(param); + animation->Update(); } catch (const std::runtime_error & e) { throw std::runtime_error("Failed to update animation: " + std::string(e.what())); } } +std::vector LEDSegment::GetAnimationFrame() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + if (default_animation_ && animation_->IsFinished()) { + return default_animation_->GetFrame(invert_led_order_); + } + + return animation_->GetFrame(invert_led_order_); +} + std::size_t LEDSegment::GetFirstLEDPosition() const { return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen; From 4358b6abb60e519e8f1270af0cf1ae6b1651e45b Mon Sep 17 00:00:00 2001 From: Dawid Date: Thu, 22 Feb 2024 14:50:33 +0000 Subject: [PATCH 05/32] add yaml_utils to panther_utils --- panther_utils/CMakeLists.txt | 7 ++ .../include/panther_utils/yaml_utils.hpp | 74 ++++++++++++++ panther_utils/package.xml | 1 + panther_utils/test/test_yaml_utils.cpp | 96 +++++++++++++++++++ 4 files changed, 178 insertions(+) create mode 100644 panther_utils/include/panther_utils/yaml_utils.hpp create mode 100644 panther_utils/test/test_yaml_utils.cpp diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index f486878f0..1443712e0 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -37,6 +37,13 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_ros_test_utils rclcpp std_msgs) + + ament_add_gtest(${PROJECT_NAME}_test_yaml_utils test/test_yaml_utils.cpp) + target_include_directories( + ${PROJECT_NAME}_test_yaml_utils + PUBLIC $ + $) + target_link_libraries(${PROJECT_NAME}_test_yaml_utils yaml-cpp) endif() ament_package() diff --git a/panther_utils/include/panther_utils/yaml_utils.hpp b/panther_utils/include/panther_utils/yaml_utils.hpp new file mode 100644 index 000000000..419050ac5 --- /dev/null +++ b/panther_utils/include/panther_utils/yaml_utils.hpp @@ -0,0 +1,74 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_UTILS_YAML_UTILS_HPP_ +#define PANTHER_UTILS_YAML_UTILS_HPP_ + +#include +#include + +#include + +namespace panther_utils +{ + +/** + * @brief Parse YAML key value from description + * + * @param description YAML description + * @param key Key name + * + * @exception std::runtime_error if key does'n exists or fails to convert key value to given type + */ +template +T GetYAMLKeyValue(const YAML::Node & description, const std::string & key) +{ + if (!description[key]) { + throw std::runtime_error("Missing '" + static_cast(key) + "' in description"); + } + + try { + return description[key].as(); + } catch (const YAML::BadConversion & e) { + throw std::runtime_error("Failed to convert '" + static_cast(key) + "' key"); + } +} + +/** + * @brief Parse YAML key value from description + * + * @param description YAML description + * @param key Key name + * @param default_value Value that will be returned if key doesn't exists + * + * @exception std::runtime_error if fails to convert key value to given type + */ +template +T GetYAMLKeyValue(const YAML::Node & description, const std::string & key, const T default_value) +{ + try { + return GetYAMLKeyValue(description, key); + } catch (const std::runtime_error & e) { + if ( + std::string(e.what()).find( + "Missing '" + static_cast(key) + "' in description") != std::string::npos) { + return default_value; + } + throw; + } +} + +} // namespace panther_utils + +#endif // PANTHER_UTILS_YAML_UTILS_HPP_ diff --git a/panther_utils/package.xml b/panther_utils/package.xml index 73c84b9b7..a0e994f30 100644 --- a/panther_utils/package.xml +++ b/panther_utils/package.xml @@ -17,6 +17,7 @@ rclcpp std_msgs + yaml-cpp ament_cmake_gtest ament_lint_auto diff --git a/panther_utils/test/test_yaml_utils.cpp b/panther_utils/test/test_yaml_utils.cpp new file mode 100644 index 000000000..2f93af0b9 --- /dev/null +++ b/panther_utils/test/test_yaml_utils.cpp @@ -0,0 +1,96 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include + +TEST(TestGetYAMLKeyValue, MissingKey) +{ + YAML::Node desc; + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [desc]() { panther_utils::GetYAMLKeyValue(desc, "key_name"); }, + "Missing 'key_name' in description")); +} + +TEST(TestGetYAMLKeyValue, ConversionFailure) +{ + YAML::Node desc; + + desc["float_key"] = 1.5; + desc["string_key"] = "string"; + desc["int_vector_key"] = "[1 2 3]"; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue(desc, "float_key"); }, "Failed to convert")); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue(desc, "string_key"); }, "Failed to convert")); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue(desc, "int_vector_key"); }, "Failed to convert")); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { panther_utils::GetYAMLKeyValue>(desc, "string_key"); }, + "Failed to convert")); +} + +TEST(TestGetYAMLKeyValue, GetKey) +{ + YAML::Node desc; + + desc["int_key"] = 2; + desc["float_key"] = 1.5; + desc["string_key"] = "string"; + + const auto int_value = panther_utils::GetYAMLKeyValue(desc, "int_key"); + EXPECT_EQ(2, int_value); + + const auto float_value = panther_utils::GetYAMLKeyValue(desc, "float_key"); + EXPECT_EQ(1.5, float_value); + + const auto str_value = panther_utils::GetYAMLKeyValue(desc, "string_key"); + EXPECT_EQ("string", str_value); +} + +TEST(TestGetYAMLKeyValue, GetVectorKey) +{ + YAML::Node desc; + + desc["int_vector_key"] = std::vector(5, 147); + + const auto value_vector = panther_utils::GetYAMLKeyValue>( + desc, "int_vector_key"); + for (auto value : value_vector) { + EXPECT_EQ(147, value); + } +} + +TEST(TestGetYAMLKeyValue, GetKeyDefaultValue) +{ + YAML::Node desc; + const int default_value = 54; + + const auto value = panther_utils::GetYAMLKeyValue(desc, "key_name", default_value); + EXPECT_EQ(default_value, value); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 63758f0e4bea5dc1351ca93a7784c7c5cb329518 Mon Sep 17 00:00:00 2001 From: Dawid Date: Thu, 22 Feb 2024 14:53:47 +0000 Subject: [PATCH 06/32] add led animation and queue --- panther_lights/CMakeLists.txt | 2 +- .../panther_lights/controller_node.hpp | 105 +++++-- .../include/panther_lights/led_segment.hpp | 13 +- panther_lights/src/controller_node.cpp | 278 +++++++++++++----- panther_lights/src/led_segment.cpp | 19 +- 5 files changed, 308 insertions(+), 109 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 79fbe785f..c1cde4625 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -44,7 +44,7 @@ add_executable( src/led_panel.cpp src/segment_converter.cpp) ament_target_dependencies(controller_node rclcpp pluginlib panther_msgs - sensor_msgs) + panther_utils sensor_msgs) target_link_libraries(controller_node yaml-cpp) add_executable(dummy_scheduler_node src/dummy_scheduler_node_main.cpp diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index d73b3f0d6..42c65f902 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -16,6 +16,7 @@ #define PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ #include +#include #include #include @@ -30,19 +31,91 @@ #include #include +#include + namespace panther_lights { using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; -struct AnimationWrapper +struct AnimationDescription +{ + std::string type; + std::vector segments; + YAML::Node animation; +}; + +struct LEDAnimationDescription { std::size_t id; std::size_t priority; std::string name; - std::vector segments; - std::vector animations; - bool repeating = false; + std::vector animations; +}; + +class LEDAnimation +{ +public: + LEDAnimation(const LEDAnimationDescription & led_animation_description); + + ~LEDAnimation() {} + + bool IsFinished(std::unordered_map> & segments); + + void ResetTime() + { + // TODO: + init_time_ = rclcpp::Time(0); + } + + std::string GetName() const { return led_animation_description_.name; } + std::size_t GetPriority() const { return led_animation_description_.priority; } + std::vector GetAnimations() const + { + return led_animation_description_.animations; + } + rclcpp::Time GetInitTime() const { return init_time_; } + float GetTimeout() const { return timeout_; } + LEDAnimationDescription GetAnimationDescription() const { return led_animation_description_; } + + bool IsRepeating() const { return repeating_; } + + void SetInitTime(const rclcpp::Time init_time) { init_time_ = init_time; } + void SetRepeating(const bool value) { repeating_ = value; } + + static constexpr char kAnimationDefaultName[] = "UNDEFINED"; + static constexpr std::size_t kAnimationDefaultPriority = 3; + static constexpr float kAnimationDefaultTimeout = 120.0f; + +private: + const LEDAnimationDescription led_animation_description_; + const float timeout_; + rclcpp::Time init_time_; + + bool repeating_; + std::string param_; +}; + +class AnimationsQueue +{ +public: + AnimationsQueue(const std::size_t max_queue_size = 5) : max_queue_size_(max_queue_size) {} + + void Put(const std::shared_ptr & animation); + std::shared_ptr Get(); + void Clear(std::size_t priority = 2); + void Remove(const std::shared_ptr & animation); + + void Validate(); + + bool HasAnimation(const std::shared_ptr & animation) const; + std::size_t GetFirstAnimationPriority() const; + + bool Empty() const { return queue_.empty(); } + +private: + std::deque> queue_; + const std::size_t max_queue_size_; }; class ControllerNode : public rclcpp::Node @@ -53,14 +126,6 @@ class ControllerNode : public rclcpp::Node ~ControllerNode() {} private: - // might be moved to separate file so animations can ues it too, or even to panther_utils - template - T GetYAMLKeyValue(const YAML::Node & description, const std::string & key) const; - - template - T GetYAMLKeyValue( - const YAML::Node & description, const std::string & key, const T default_value) const; - void DeclareParameters(); void LoadParameters(); @@ -113,7 +178,11 @@ class ControllerNode : public rclcpp::Node * * @exception std::runtime_error animation with give ID does not exists or (to be updated) */ - void SetAnimation(const std::size_t animation_id); + void SetAnimation(const std::size_t animation_id, const bool repeating); + + void UpdateAndPublishAnimation(); + void AddAnimationToQueue(const std::size_t animation_id, const bool repeating); + void SetLEDAnimation(const std::shared_ptr & led_animation); void PublishPanelFrame(const std::size_t channel); void SetLEDAnimationCB( @@ -121,20 +190,22 @@ class ControllerNode : public rclcpp::Node SetLEDAnimationSrv::Response::SharedPtr response); void ControllerTimerCB(); - static constexpr std::uint8_t kDefaultAnimaitonPriority = 3; - std::unordered_map> led_panels_; std::unordered_map::SharedPtr> panel_publishers_; std::unordered_map> segments_; std::unordered_map> segments_map_; - std::unordered_map animations_; + std::unordered_map> animations_; std::shared_ptr segment_converter_; rclcpp::Service::SharedPtr set_led_animation_server_; rclcpp::TimerBase::SharedPtr controller_timer_; - std::shared_ptr current_animation_; + std::shared_ptr current_animation_; + std::shared_ptr default_animation_; + std::unique_ptr animation_queue_; + + std::mutex queue_mtx_; }; } // namespace panther_lights diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 696d91bb4..35885d3ba 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -58,15 +58,25 @@ class LEDSegment * @exception std::runtime_error if 'type' key is missing, given pluginlib fails to load or * animation fails to initialize */ - void SetAnimation(const YAML::Node & animation_description, const bool repeating); + void SetAnimation( + const std::string & type, const YAML::Node & animation_description, const bool repeating); /** * @brief Update animation frame * + * @param param optional parameter to pass to animation when updating + * * @exception std::runtime_error if fails to update animation */ void UpdateAnimation(const std::string & param = ""); + /** + * @brief Check if animation is finished. This does not return state of the default animation + * + * @return True if animation is finished, false otherwise + */ + bool IsAnimationFinished() const { return animaiton_finished_; } + /** * @brief Get current animation frame * @@ -83,6 +93,7 @@ class LEDSegment private: const float controller_frequency_; bool invert_led_order_ = false; + bool animaiton_finished_ = true; std::size_t channel_; std::size_t first_led_iterator_; std::size_t last_led_iterator_; diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 6779f1917..634b591f0 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -34,6 +34,7 @@ #include #include #include +#include namespace panther_lights { @@ -41,6 +42,105 @@ namespace panther_lights using std::placeholders::_1; using std::placeholders::_2; +LEDAnimation::LEDAnimation(const LEDAnimationDescription & led_animation_description) +: led_animation_description_(led_animation_description), + timeout_(kAnimationDefaultTimeout), + init_time_(rclcpp::Time(0)), + repeating_(false), + param_("") +{ +} + +bool LEDAnimation::IsFinished( + std::unordered_map> & segments) +{ + for (auto & animation : led_animation_description_.animations) { + for (auto & segment : animation.segments) { + if (!segments.at(segment)->IsAnimationFinished()) { + return false; + } + } + } + + return true; +} + +void AnimationsQueue::Put(const std::shared_ptr & animation) +{ + if (animation->GetPriority() == 1) { + Clear(); + } + Validate(); + + if (queue_.size() == max_queue_size_) { + throw std::runtime_error("Animation queue overloaded"); + } + + queue_.push_back(animation); + std::sort( + queue_.begin(), queue_.end(), + [](const std::shared_ptr & a, const std::shared_ptr & b) { + return a->GetPriority() < b->GetPriority() || + (a->GetPriority() == b->GetPriority() && a->GetInitTime() < b->GetInitTime()); + }); + + for (auto & a : queue_) { + } +} + +std::shared_ptr AnimationsQueue::Get() +{ + if (!queue_.empty()) { + const auto animation = queue_.front(); + queue_.pop_front(); + return animation; + } + throw std::runtime_error("Queue empty"); +} + +void AnimationsQueue::Clear(std::size_t priority) +{ + const auto new_end = std::remove_if( + queue_.begin(), queue_.end(), [priority](const std::shared_ptr & animation) { + return animation->GetPriority() >= priority; + }); + queue_.erase(new_end, queue_.end()); +} + +void AnimationsQueue::Remove(const std::shared_ptr & animation) +{ + auto it = std::find(queue_.begin(), queue_.end(), animation); + if (it != queue_.end()) { + queue_.erase(it); + } +} + +bool AnimationsQueue::HasAnimation(const std::shared_ptr & animation) const +{ + return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); +} + +void AnimationsQueue::Validate() +{ + const auto current_time = rclcpp::Time(0); // TODO + for (auto it = queue_.begin(); it != queue_.end();) { + if ((current_time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { + // TODO: log info - animation timeout + it = queue_.erase(it); + } else { + ++it; + } + } +} + +std::size_t AnimationsQueue::GetFirstAnimationPriority() const +{ + if (!Empty()) { + return queue_.front()->GetPriority(); + } + return LEDAnimation::kAnimationDefaultPriority; +} + ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { @@ -61,6 +161,8 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node segment_converter_ = std::make_shared(); + animation_queue_ = std::make_unique(10); + set_led_animation_server_ = this->create_service( "lights/controller/set/animation", std::bind(&ControllerNode::SetLEDAnimationCB, this, _1, _2)); @@ -74,36 +176,6 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node // before? } -template -T ControllerNode::GetYAMLKeyValue(const YAML::Node & description, const std::string & key) const -{ - if (!description[key]) { - throw std::runtime_error("Missing '" + static_cast(key) + "' in description"); - } - - try { - return description[key].as(); - } catch (const YAML::BadConversion & e) { - throw std::runtime_error("Failed to convert '" + static_cast(key) + "' key"); - } -} - -template -T ControllerNode::GetYAMLKeyValue( - const YAML::Node & description, const std::string & key, const T default_value) const -{ - try { - return GetYAMLKeyValue(description, key); - } catch (const std::runtime_error & e) { - if ( - std::string(e.what()).find( - "Missing '" + static_cast(key) + "' in description") != std::string::npos) { - return default_value; - } - throw; - } -} - void ControllerNode::DeclareParameters() { // this->declare_parameter("led_config_file"); @@ -119,8 +191,9 @@ void ControllerNode::InitializeLEDPanels(const YAML::Node & panels_description) RCLCPP_INFO(this->get_logger(), "Initializing LED panels"); for (auto & panel : panels_description.as>()) { - const auto channel = GetYAMLKeyValue(panel, "channel"); - const auto number_of_leds = GetYAMLKeyValue(panel, "number_of_leds"); + const auto channel = panther_utils::GetYAMLKeyValue(panel, "channel"); + const auto number_of_leds = panther_utils::GetYAMLKeyValue( + panel, "number_of_leds"); const auto result = led_panels_.emplace(channel, std::make_unique(number_of_leds)); if (!result.second) { @@ -145,7 +218,7 @@ void ControllerNode::InitializeLEDSegments( { RCLCPP_INFO(this->get_logger(), "Initializing LED segments"); for (auto & segment : segments_description.as>()) { - const auto segment_name = GetYAMLKeyValue(segment, "name"); + const auto segment_name = panther_utils::GetYAMLKeyValue(segment, "name"); try { const auto result = segments_.emplace( @@ -178,59 +251,43 @@ void ControllerNode::LoadAnimations(const YAML::Node & animations_description) { RCLCPP_INFO(this->get_logger(), "Loading LED animations"); for (auto & animation_description : animations_description.as>()) { - AnimationWrapper animation; - - animation.id = GetYAMLKeyValue(animation_description, "id"); - animation.name = GetYAMLKeyValue(animation_description, "name"); - animation.priority = GetYAMLKeyValue( - animation_description, "priority", kDefaultAnimaitonPriority); - animation.animations = GetYAMLKeyValue>( - animation_description, "animations"); - - const auto result = animations_.emplace(animation.id, animation); - if (!result.second) { - throw std::runtime_error( - "Failed to load '" + animation.name + - "' animation: Animation with given ID already exists."); - } - } - - RCLCPP_INFO(this->get_logger(), "Animations Successfully loaded"); -} - -void ControllerNode::SetAnimation(const std::size_t animation_id) -{ - if (animations_.find(animation_id) == animations_.end()) { - throw std::runtime_error("No animation with ID: " + std::to_string(animation_id)); - } - - const auto animation = animations_.at(animation_id); - - for (auto & segment_animation : animation.animations) { - auto segments_group = GetYAMLKeyValue(segment_animation, "segments"); + LEDAnimationDescription led_animation_desc; - std::vector segments; try { - segments = segments_map_.at(segments_group); - } catch (const std::out_of_range & e) { - throw std::out_of_range("No segment group with name: " + segments_group); - } - - for (auto & segment : segments) { - if (segments_.find(segment) == segments_.end()) { - throw std::runtime_error("No segment with name: " + segment); + led_animation_desc.name = panther_utils::GetYAMLKeyValue( + animation_description, "name", LEDAnimation::kAnimationDefaultName); + led_animation_desc.id = panther_utils::GetYAMLKeyValue( + animation_description, "id"); + led_animation_desc.priority = panther_utils::GetYAMLKeyValue( + animation_description, "priority", LEDAnimation::kAnimationDefaultPriority); + + auto animations = panther_utils::GetYAMLKeyValue>( + animation_description, "animations"); + for (auto & animation : animations) { + AnimationDescription animation_desc; + animation_desc.type = panther_utils::GetYAMLKeyValue(animation, "type"); + animation_desc.animation = panther_utils::GetYAMLKeyValue( + animation, "animation"); + + auto segments_group = panther_utils::GetYAMLKeyValue(animation, "segments"); + animation_desc.segments = segments_map_.at(segments_group); + + led_animation_desc.animations.push_back(animation_desc); } - try { - segments_.at(segment)->SetAnimation(segment_animation); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to set '" + animation.name + "' animation: " + std::string(e.what())); + const auto result = animations_.emplace( + led_animation_desc.id, std::make_shared(led_animation_desc)); + if (!result.second) { + throw std::runtime_error("Animation with given ID already exists."); } + + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to load '" + led_animation_desc.name + "' animation: " + std::string(e.what())); } } - current_animation_ = std::make_shared(animation); + RCLCPP_INFO(this->get_logger(), "Animations Successfully loaded"); } void ControllerNode::SetLEDAnimationCB( @@ -238,7 +295,8 @@ void ControllerNode::SetLEDAnimationCB( SetLEDAnimationSrv::Response::SharedPtr response) { try { - SetAnimation(request->animation.id); + // request->animation.param; TODO + AddAnimationToQueue(request->animation.id, request->repeating); response->success = true; } catch (const std::exception & e) { response->success = false; @@ -265,11 +323,39 @@ void ControllerNode::PublishPanelFrame(const std::size_t channel) void ControllerNode::ControllerTimerCB() { + std::lock_guard lck_g(queue_mtx_); + + auto brightness = 255; + + if (!animation_queue_->Empty()) { + std::cout << "not emptinga" << std::endl; + current_animation_ = animation_queue_->Get(); + SetLEDAnimation(current_animation_); + + // const auto segments = current_animation_->GetAnimation().segments; + + // auto result = std::all_of(segments.begin(), segments.end(), [&](const std::string & segment) + // { + // return segments_.at(segment)->IsAnimationFinished(); + // }); + // if (result) { + // } + } + if (!current_animation_) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for animation"); return; } + // if (current_animation_->IsFinished(segments_)) { + // std::cout << "animation finished" << std::endl; + // } + + UpdateAndPublishAnimation(); +} + +void ControllerNode::UpdateAndPublishAnimation() +{ std::vector> segments_vec; for (auto & [segment_name, segment] : segments_) { @@ -295,4 +381,38 @@ void ControllerNode::ControllerTimerCB() } } +void ControllerNode::AddAnimationToQueue(const std::size_t animation_id, const bool repeating) +{ + if (animations_.find(animation_id) == animations_.end()) { + throw std::runtime_error("No animation with ID: " + std::to_string(animation_id)); + } + + const auto animation = animations_.at(animation_id); + animation->SetRepeating(repeating); + // animation->SetParam(param); + animation_queue_->Put(animation); +} + +void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_animation) +{ + const auto animations = led_animation->GetAnimations(); + for (auto & animation : animations) { + for (auto & segment : animation.segments) { + if (segments_.find(segment) == segments_.end()) { + throw std::runtime_error("No segment with name: " + segment); + } + + try { + segments_.at(segment)->SetAnimation( + animation.type, animation.animation, led_animation->IsRepeating()); + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to set '" + led_animation->GetName() + "' animation: " + std::string(e.what())); + } + } + } + + current_animation_ = std::move(led_animation); +} + } // namespace panther_lights diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index 50d44be9e..997150762 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -74,14 +74,9 @@ LEDSegment::~LEDSegment() animation_loader_.reset(); } -void LEDSegment::SetAnimation(const YAML::Node & animation_description, const bool repeating) +void LEDSegment::SetAnimation( + const std::string & type, const YAML::Node & animation_description, const bool repeating) { - if (!animation_description["type"]) { - throw std::runtime_error("Missing 'type' in animation description"); - } - - auto type = animation_description["type"].as(); - std::shared_ptr animation; try { @@ -99,6 +94,7 @@ void LEDSegment::SetAnimation(const YAML::Node & animation_description, const bo } animation_ = std::move(animation); + animaiton_finished_ = false; if (repeating) { default_animation_ = animation_; @@ -115,11 +111,12 @@ void LEDSegment::UpdateAnimation(const std::string & param) std::shared_ptr animation; - if (animation_->IsFinished() && default_animation_) { - animation = default_animation_; - if (animation->IsFinished()) { - animation->Reset(); + if (default_animation_ && animation_->IsFinished()) { + animaiton_finished_ = true; + if (default_animation_->IsFinished()) { + default_animation_->Reset(); } + animation = default_animation_; } else { animation = animation_; } From 22820624826072a6492e31e92a8c7840b7a11df0 Mon Sep 17 00:00:00 2001 From: Dawid Date: Fri, 23 Feb 2024 10:59:06 +0000 Subject: [PATCH 07/32] Fix queuing --- .../panther_lights/controller_node.hpp | 10 ++++++++ panther_lights/src/controller_node.cpp | 24 +++++++------------ panther_lights/src/led_segment.cpp | 21 ++++++++-------- 3 files changed, 28 insertions(+), 27 deletions(-) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 42c65f902..398d65200 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -113,6 +113,14 @@ class AnimationsQueue bool Empty() const { return queue_.empty(); } + void Print() + { + std::cout << "--------" << std::endl; + for (auto & anim : queue_) { + std::cout << anim->GetName() << std::endl; + } + } + private: std::deque> queue_; const std::size_t max_queue_size_; @@ -206,6 +214,8 @@ class ControllerNode : public rclcpp::Node std::unique_ptr animation_queue_; std::mutex queue_mtx_; + + bool animation_finished_ = true; }; } // namespace panther_lights diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 634b591f0..98ea158c5 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -83,9 +83,6 @@ void AnimationsQueue::Put(const std::shared_ptr & animation) return a->GetPriority() < b->GetPriority() || (a->GetPriority() == b->GetPriority() && a->GetInitTime() < b->GetInitTime()); }); - - for (auto & a : queue_) { - } } std::shared_ptr AnimationsQueue::Get() @@ -327,19 +324,12 @@ void ControllerNode::ControllerTimerCB() auto brightness = 255; - if (!animation_queue_->Empty()) { - std::cout << "not emptinga" << std::endl; - current_animation_ = animation_queue_->Get(); - SetLEDAnimation(current_animation_); - - // const auto segments = current_animation_->GetAnimation().segments; + if (animation_finished_) { + animation_queue_->Validate(); - // auto result = std::all_of(segments.begin(), segments.end(), [&](const std::string & segment) - // { - // return segments_.at(segment)->IsAnimationFinished(); - // }); - // if (result) { - // } + if (!animation_queue_->Empty()) { + SetLEDAnimation(animation_queue_->Get()); + } } if (!current_animation_) { @@ -352,6 +342,8 @@ void ControllerNode::ControllerTimerCB() // } UpdateAndPublishAnimation(); + + animation_finished_ = current_animation_->IsFinished(segments_); } void ControllerNode::UpdateAndPublishAnimation() @@ -365,7 +357,6 @@ void ControllerNode::UpdateAndPublishAnimation() RCLCPP_WARN( this->get_logger(), "Failed to update animation on %s segment: %s", segment_name.c_str(), e.what()); - return; } } @@ -391,6 +382,7 @@ void ControllerNode::AddAnimationToQueue(const std::size_t animation_id, const b animation->SetRepeating(repeating); // animation->SetParam(param); animation_queue_->Put(animation); + animation_queue_->Print(); } void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_animation) diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index 997150762..c8bca0c25 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -109,21 +109,20 @@ void LEDSegment::UpdateAnimation(const std::string & param) throw std::runtime_error("Segment animation not defined"); } - std::shared_ptr animation; - - if (default_animation_ && animation_->IsFinished()) { + if (animation_->IsFinished()) { animaiton_finished_ = true; - if (default_animation_->IsFinished()) { - default_animation_->Reset(); - } - animation = default_animation_; - } else { - animation = animation_; + } + + std::shared_ptr animation_to_update = + animaiton_finished_ && default_animation_ ? default_animation_ : animation_; + + if (animaiton_finished_ && default_animation_ && default_animation_->IsFinished()) { + default_animation_->Reset(); } try { - animation->SetParam(param); - animation->Update(); + animation_to_update->SetParam(param); + animation_to_update->Update(); } catch (const std::runtime_error & e) { throw std::runtime_error("Failed to update animation: " + std::string(e.what())); } From 4aafb1b917865d077362bc159333e49a5681f69f Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 26 Feb 2024 08:48:53 +0000 Subject: [PATCH 08/32] fix bug --- .../panther_lights/animation/animation.hpp | 37 ++++++++++--------- .../include/panther_lights/led_segment.hpp | 4 +- panther_lights/src/led_segment.cpp | 13 ++++--- 3 files changed, 28 insertions(+), 26 deletions(-) diff --git a/panther_lights/include/panther_lights/animation/animation.hpp b/panther_lights/include/panther_lights/animation/animation.hpp index 22863da96..6546e95f5 100644 --- a/panther_lights/include/panther_lights/animation/animation.hpp +++ b/panther_lights/include/panther_lights/animation/animation.hpp @@ -92,31 +92,31 @@ class Animation */ void Update() { - if (current_cycle_ < loops_) { - auto frame = UpdateFrame(); + if (current_cycle_ >= loops_) { + std::fill(frame_.begin(), frame_.end(), 0); + return; + } - if (frame.size() != num_led_ * kRGBAColorLen) { - throw std::runtime_error( - "Invalid frame size. Check animation UpdateFrame() method implementation"); - } + auto frame = UpdateFrame(); - anim_iteration_++; - progress_ = float(anim_iteration_ + anim_len_ * current_cycle_) / full_anim_len_; + if (frame.size() != num_led_ * kRGBAColorLen) { + throw std::runtime_error( + "Invalid frame size. Check animation UpdateFrame() method implementation"); + } - if (anim_iteration_ >= anim_len_) { - anim_iteration_ = 0; - current_cycle_++; - } + anim_iteration_++; + progress_ = float(anim_iteration_ + anim_len_ * current_cycle_) / full_anim_len_; - if (current_cycle_ >= loops_) { - finished_ = true; - } + if (anim_iteration_ >= anim_len_) { + anim_iteration_ = 0; + current_cycle_++; + } - frame_ = frame; - return; + if (current_cycle_ >= loops_) { + finished_ = true; } - std::fill(frame_.begin(), frame_.end(), 0); + frame_ = frame; } /** @@ -129,6 +129,7 @@ class Animation current_cycle_ = 0; finished_ = false; progress_ = 0.0; + std::fill(frame_.begin(), frame_.end(), 0); } /** diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 35885d3ba..2e40463a0 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -75,7 +75,7 @@ class LEDSegment * * @return True if animation is finished, false otherwise */ - bool IsAnimationFinished() const { return animaiton_finished_; } + bool IsAnimationFinished() const { return animation_finished_; } /** * @brief Get current animation frame @@ -93,7 +93,7 @@ class LEDSegment private: const float controller_frequency_; bool invert_led_order_ = false; - bool animaiton_finished_ = true; + bool animation_finished_ = true; std::size_t channel_; std::size_t first_led_iterator_; std::size_t last_led_iterator_; diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index c8bca0c25..bd4d68688 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -94,11 +94,12 @@ void LEDSegment::SetAnimation( } animation_ = std::move(animation); - animaiton_finished_ = false; + animation_finished_ = false; if (repeating) { default_animation_ = animation_; - } else if (default_animation_) { + } + if (default_animation_) { default_animation_->Reset(); } } @@ -110,13 +111,13 @@ void LEDSegment::UpdateAnimation(const std::string & param) } if (animation_->IsFinished()) { - animaiton_finished_ = true; + animation_finished_ = true; } std::shared_ptr animation_to_update = - animaiton_finished_ && default_animation_ ? default_animation_ : animation_; + animation_finished_ && default_animation_ ? default_animation_ : animation_; - if (animaiton_finished_ && default_animation_ && default_animation_->IsFinished()) { + if (animation_finished_ && default_animation_ && default_animation_->IsFinished()) { default_animation_->Reset(); } @@ -134,7 +135,7 @@ std::vector LEDSegment::GetAnimationFrame() const throw std::runtime_error("Segment animation not defined"); } - if (default_animation_ && animation_->IsFinished()) { + if (default_animation_ && animation_finished_) { return default_animation_->GetFrame(invert_led_order_); } From a46ad4cb0d7a60cb38c7ca3fcb4485b4014fe80a Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 26 Feb 2024 10:34:47 +0000 Subject: [PATCH 09/32] priority and timeout queue validation --- .../panther_lights/controller_node.hpp | 36 +++---- .../include/panther_lights/led_segment.hpp | 16 +++ panther_lights/src/controller_node.cpp | 97 +++++++++++++------ panther_lights/src/led_segment.cpp | 18 ++++ 4 files changed, 119 insertions(+), 48 deletions(-) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 398d65200..338bf89d5 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -48,52 +48,54 @@ struct AnimationDescription struct LEDAnimationDescription { std::size_t id; - std::size_t priority; + std::uint8_t priority; std::string name; + float timeout; std::vector animations; }; class LEDAnimation { public: - LEDAnimation(const LEDAnimationDescription & led_animation_description); + LEDAnimation( + const LEDAnimationDescription & led_animation_description, + const std::unordered_map> & segments, + const rclcpp::Time & init_time); ~LEDAnimation() {} - bool IsFinished(std::unordered_map> & segments); + bool IsFinished(); - void ResetTime() - { - // TODO: - init_time_ = rclcpp::Time(0); - } + void ResetTime(const rclcpp::Time & init_time) { init_time_ = init_time; } std::string GetName() const { return led_animation_description_.name; } - std::size_t GetPriority() const { return led_animation_description_.priority; } + std::uint8_t GetPriority() const { return led_animation_description_.priority; } std::vector GetAnimations() const { return led_animation_description_.animations; } rclcpp::Time GetInitTime() const { return init_time_; } - float GetTimeout() const { return timeout_; } + float GetTimeout() const { return led_animation_description_.timeout; } LEDAnimationDescription GetAnimationDescription() const { return led_animation_description_; } + float GetProgress() const; + void Reset() const; bool IsRepeating() const { return repeating_; } void SetInitTime(const rclcpp::Time init_time) { init_time_ = init_time; } void SetRepeating(const bool value) { repeating_ = value; } - static constexpr char kAnimationDefaultName[] = "UNDEFINED"; - static constexpr std::size_t kAnimationDefaultPriority = 3; - static constexpr float kAnimationDefaultTimeout = 120.0f; + static constexpr char kDefaultName[] = "UNDEFINED"; + static constexpr std::uint8_t kDefaultPriority = 3; + static constexpr float kDefaultTimeout = 120.0f; private: const LEDAnimationDescription led_animation_description_; - const float timeout_; rclcpp::Time init_time_; bool repeating_; std::string param_; + std::vector> animation_segments_; }; class AnimationsQueue @@ -101,12 +103,12 @@ class AnimationsQueue public: AnimationsQueue(const std::size_t max_queue_size = 5) : max_queue_size_(max_queue_size) {} - void Put(const std::shared_ptr & animation); + void Put(const std::shared_ptr & animation, const rclcpp::Time & time); std::shared_ptr Get(); void Clear(std::size_t priority = 2); void Remove(const std::shared_ptr & animation); - void Validate(); + void Validate(const rclcpp::Time & time); bool HasAnimation(const std::shared_ptr & animation) const; std::size_t GetFirstAnimationPriority() const; @@ -203,7 +205,7 @@ class ControllerNode : public rclcpp::Node panel_publishers_; std::unordered_map> segments_; std::unordered_map> segments_map_; - std::unordered_map> animations_; + std::unordered_map animations_descriptions_; std::shared_ptr segment_converter_; rclcpp::Service::SharedPtr set_led_animation_server_; diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 2e40463a0..94f54eb3a 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -86,6 +86,22 @@ class LEDSegment */ std::vector GetAnimationFrame() const; + /** + * @brief Get current animation progress + * + * @return Current animation progress + * + * @exception std::runtime_error if segment animation is not defined + */ + float GetAnimationProgress() const; + + /** + * @brief Reset current animation + * + * @exception std::runtime_error if segment animation is not defined + */ + void ResetAnimation() const; + std::size_t GetFirstLEDPosition() const; std::size_t GetChannel() const { return channel_; } diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 98ea158c5..633221a3f 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -42,35 +42,56 @@ namespace panther_lights using std::placeholders::_1; using std::placeholders::_2; -LEDAnimation::LEDAnimation(const LEDAnimationDescription & led_animation_description) +LEDAnimation::LEDAnimation( + const LEDAnimationDescription & led_animation_description, + const std::unordered_map> & segments, + const rclcpp::Time & init_time) : led_animation_description_(led_animation_description), - timeout_(kAnimationDefaultTimeout), - init_time_(rclcpp::Time(0)), + init_time_(init_time), repeating_(false), param_("") -{ -} - -bool LEDAnimation::IsFinished( - std::unordered_map> & segments) { for (auto & animation : led_animation_description_.animations) { for (auto & segment : animation.segments) { - if (!segments.at(segment)->IsAnimationFinished()) { - return false; - } + animation_segments_.push_back(segments.at(segment)); } } +} + +bool LEDAnimation::IsFinished() +{ + return std::all_of( + animation_segments_.begin(), animation_segments_.end(), + [](const std::shared_ptr & segment) { return segment->IsAnimationFinished(); }); +} + +float LEDAnimation::GetProgress() const +{ + float progress = 1.0f; + std::for_each( + animation_segments_.begin(), animation_segments_.end(), + [&progress](const std::shared_ptr & segment) { + auto anim_progress = segment->GetAnimationProgress(); + progress = anim_progress < progress ? anim_progress : progress; + }); - return true; + return progress; } -void AnimationsQueue::Put(const std::shared_ptr & animation) +void LEDAnimation::Reset() const +{ + std::for_each( + animation_segments_.begin(), animation_segments_.end(), + [](const std::shared_ptr & segment) { segment->ResetAnimation(); }); +} + +void AnimationsQueue::Put( + const std::shared_ptr & animation, const rclcpp::Time & time) { if (animation->GetPriority() == 1) { Clear(); } - Validate(); + Validate(time); if (queue_.size() == max_queue_size_) { throw std::runtime_error("Animation queue overloaded"); @@ -117,11 +138,10 @@ bool AnimationsQueue::HasAnimation(const std::shared_ptr & animati return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); } -void AnimationsQueue::Validate() +void AnimationsQueue::Validate(const rclcpp::Time & time) { - const auto current_time = rclcpp::Time(0); // TODO for (auto it = queue_.begin(); it != queue_.end();) { - if ((current_time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { + if ((time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { // TODO: log info - animation timeout it = queue_.erase(it); } else { @@ -135,7 +155,7 @@ std::size_t AnimationsQueue::GetFirstAnimationPriority() const if (!Empty()) { return queue_.front()->GetPriority(); } - return LEDAnimation::kAnimationDefaultPriority; + return LEDAnimation::kDefaultPriority; } ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) @@ -252,11 +272,13 @@ void ControllerNode::LoadAnimations(const YAML::Node & animations_description) try { led_animation_desc.name = panther_utils::GetYAMLKeyValue( - animation_description, "name", LEDAnimation::kAnimationDefaultName); + animation_description, "name", LEDAnimation::kDefaultName); led_animation_desc.id = panther_utils::GetYAMLKeyValue( animation_description, "id"); led_animation_desc.priority = panther_utils::GetYAMLKeyValue( - animation_description, "priority", LEDAnimation::kAnimationDefaultPriority); + animation_description, "priority", LEDAnimation::kDefaultPriority); + led_animation_desc.timeout = panther_utils::GetYAMLKeyValue( + animation_description, "timeout", LEDAnimation::kDefaultTimeout); auto animations = panther_utils::GetYAMLKeyValue>( animation_description, "animations"); @@ -272,8 +294,8 @@ void ControllerNode::LoadAnimations(const YAML::Node & animations_description) led_animation_desc.animations.push_back(animation_desc); } - const auto result = animations_.emplace( - led_animation_desc.id, std::make_shared(led_animation_desc)); + const auto result = animations_descriptions_.emplace( + led_animation_desc.id, led_animation_desc); if (!result.second) { throw std::runtime_error("Animation with given ID already exists."); } @@ -325,25 +347,35 @@ void ControllerNode::ControllerTimerCB() auto brightness = 255; if (animation_finished_) { - animation_queue_->Validate(); + animation_queue_->Validate(this->get_clock()->now()); if (!animation_queue_->Empty()) { - SetLEDAnimation(animation_queue_->Get()); + try { + SetLEDAnimation(animation_queue_->Get()); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to Set LED animation: %s", e.what()); + } } } + // if(curent_animation) // optional if (!current_animation_) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for animation"); return; } - // if (current_animation_->IsFinished(segments_)) { - // std::cout << "animation finished" << std::endl; - // } + if (current_animation_->GetPriority() > animation_queue_->GetFirstAnimationPriority()) { + if (current_animation_->GetProgress() < 0.65f) { + current_animation_->Reset(); + animation_queue_->Put(current_animation_, this->get_clock()->now()); + } + animation_finished_ = true; + return; + } UpdateAndPublishAnimation(); - animation_finished_ = current_animation_->IsFinished(segments_); + animation_finished_ = current_animation_->IsFinished(); } void ControllerNode::UpdateAndPublishAnimation() @@ -374,14 +406,16 @@ void ControllerNode::UpdateAndPublishAnimation() void ControllerNode::AddAnimationToQueue(const std::size_t animation_id, const bool repeating) { - if (animations_.find(animation_id) == animations_.end()) { + if (animations_descriptions_.find(animation_id) == animations_descriptions_.end()) { throw std::runtime_error("No animation with ID: " + std::to_string(animation_id)); } - const auto animation = animations_.at(animation_id); + auto animation_description = animations_descriptions_.at(animation_id); + auto animation = std::make_shared( + animation_description, segments_, this->get_clock()->now()); animation->SetRepeating(repeating); // animation->SetParam(param); - animation_queue_->Put(animation); + animation_queue_->Put(animation, this->get_clock()->now()); animation_queue_->Print(); } @@ -404,6 +438,7 @@ void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_a } } + current_animation_.reset(); current_animation_ = std::move(led_animation); } diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index bd4d68688..00eab7336 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -142,6 +142,24 @@ std::vector LEDSegment::GetAnimationFrame() const return animation_->GetFrame(invert_led_order_); } +float LEDSegment::GetAnimationProgress() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + return animation_->GetProgress(); +} + +void LEDSegment::ResetAnimation() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + animation_->Reset(); +} + std::size_t LEDSegment::GetFirstLEDPosition() const { return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen; From f4b1631cfac8eb4f6dda2d8ec9bb58e9aa5565df Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 26 Feb 2024 10:50:31 +0000 Subject: [PATCH 10/32] move queue to separate file --- panther_lights/CMakeLists.txt | 2 +- .../panther_lights/controller_node.hpp | 95 +---------- .../panther_lights/led_animations_queue.hpp | 126 +++++++++++++++ panther_lights/src/controller_node.cpp | 133 ++-------------- panther_lights/src/led_animations_queue.cpp | 147 ++++++++++++++++++ 5 files changed, 285 insertions(+), 218 deletions(-) create mode 100644 panther_lights/include/panther_lights/led_animations_queue.hpp create mode 100644 panther_lights/src/led_animations_queue.cpp diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index c1cde4625..3791d8ab9 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -41,7 +41,7 @@ ament_target_dependencies( add_executable( controller_node src/controller_node_main.cpp src/controller_node.cpp src/led_segment.cpp - src/led_panel.cpp src/segment_converter.cpp) + src/led_panel.cpp src/segment_converter.cpp src/led_animations_queue.cpp) ament_target_dependencies(controller_node rclcpp pluginlib panther_msgs panther_utils sensor_msgs) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 338bf89d5..1726e54bf 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -29,8 +29,8 @@ #include #include +#include #include - #include namespace panther_lights @@ -38,96 +38,6 @@ namespace panther_lights using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; -struct AnimationDescription -{ - std::string type; - std::vector segments; - YAML::Node animation; -}; - -struct LEDAnimationDescription -{ - std::size_t id; - std::uint8_t priority; - std::string name; - float timeout; - std::vector animations; -}; - -class LEDAnimation -{ -public: - LEDAnimation( - const LEDAnimationDescription & led_animation_description, - const std::unordered_map> & segments, - const rclcpp::Time & init_time); - - ~LEDAnimation() {} - - bool IsFinished(); - - void ResetTime(const rclcpp::Time & init_time) { init_time_ = init_time; } - - std::string GetName() const { return led_animation_description_.name; } - std::uint8_t GetPriority() const { return led_animation_description_.priority; } - std::vector GetAnimations() const - { - return led_animation_description_.animations; - } - rclcpp::Time GetInitTime() const { return init_time_; } - float GetTimeout() const { return led_animation_description_.timeout; } - LEDAnimationDescription GetAnimationDescription() const { return led_animation_description_; } - float GetProgress() const; - void Reset() const; - - bool IsRepeating() const { return repeating_; } - - void SetInitTime(const rclcpp::Time init_time) { init_time_ = init_time; } - void SetRepeating(const bool value) { repeating_ = value; } - - static constexpr char kDefaultName[] = "UNDEFINED"; - static constexpr std::uint8_t kDefaultPriority = 3; - static constexpr float kDefaultTimeout = 120.0f; - -private: - const LEDAnimationDescription led_animation_description_; - rclcpp::Time init_time_; - - bool repeating_; - std::string param_; - std::vector> animation_segments_; -}; - -class AnimationsQueue -{ -public: - AnimationsQueue(const std::size_t max_queue_size = 5) : max_queue_size_(max_queue_size) {} - - void Put(const std::shared_ptr & animation, const rclcpp::Time & time); - std::shared_ptr Get(); - void Clear(std::size_t priority = 2); - void Remove(const std::shared_ptr & animation); - - void Validate(const rclcpp::Time & time); - - bool HasAnimation(const std::shared_ptr & animation) const; - std::size_t GetFirstAnimationPriority() const; - - bool Empty() const { return queue_.empty(); } - - void Print() - { - std::cout << "--------" << std::endl; - for (auto & anim : queue_) { - std::cout << anim->GetName() << std::endl; - } - } - -private: - std::deque> queue_; - const std::size_t max_queue_size_; -}; - class ControllerNode : public rclcpp::Node { public: @@ -212,8 +122,7 @@ class ControllerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr controller_timer_; std::shared_ptr current_animation_; - std::shared_ptr default_animation_; - std::unique_ptr animation_queue_; + std::unique_ptr animations_queue_; std::mutex queue_mtx_; diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp new file mode 100644 index 000000000..1c138a542 --- /dev/null +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -0,0 +1,126 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ +#define PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + +namespace panther_lights +{ + +struct AnimationDescription +{ + std::string type; + std::vector segments; + YAML::Node animation; +}; + +struct LEDAnimationDescription +{ + std::size_t id; + std::uint8_t priority; + std::string name; + float timeout; + std::vector animations; +}; + +class LEDAnimation +{ +public: + LEDAnimation( + const LEDAnimationDescription & led_animation_description, + const std::unordered_map> & segments, + const rclcpp::Time & init_time); + + ~LEDAnimation() {} + + bool IsFinished(); + + void ResetTime(const rclcpp::Time & init_time) { init_time_ = init_time; } + + std::string GetName() const { return led_animation_description_.name; } + std::uint8_t GetPriority() const { return led_animation_description_.priority; } + std::vector GetAnimations() const + { + return led_animation_description_.animations; + } + rclcpp::Time GetInitTime() const { return init_time_; } + float GetTimeout() const { return led_animation_description_.timeout; } + LEDAnimationDescription GetAnimationDescription() const { return led_animation_description_; } + float GetProgress() const; + void Reset() const; + + bool IsRepeating() const { return repeating_; } + + void SetInitTime(const rclcpp::Time init_time) { init_time_ = init_time; } + void SetRepeating(const bool value) { repeating_ = value; } + + static constexpr char kDefaultName[] = "UNDEFINED"; + static constexpr std::uint8_t kDefaultPriority = 3; + static constexpr float kDefaultTimeout = 120.0f; + +private: + const LEDAnimationDescription led_animation_description_; + rclcpp::Time init_time_; + + bool repeating_; + std::string param_; + std::vector> animation_segments_; +}; + +class LEDAnimationsQueue +{ +public: + LEDAnimationsQueue(const std::size_t max_queue_size = 5) : max_queue_size_(max_queue_size) {} + + void Put(const std::shared_ptr & animation, const rclcpp::Time & time); + std::shared_ptr Get(); + void Clear(std::size_t priority = 2); + void Remove(const std::shared_ptr & animation); + + void Validate(const rclcpp::Time & time); + + bool HasAnimation(const std::shared_ptr & animation) const; + std::size_t GetFirstAnimationPriority() const; + + bool Empty() const { return queue_.empty(); } + + void Print() + { + std::cout << "--------" << std::endl; + for (auto & anim : queue_) { + std::cout << anim->GetName() << std::endl; + } + } + +private: + std::deque> queue_; + const std::size_t max_queue_size_; +}; + +} // namespace panther_lights + +#endif // PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 633221a3f..4026898d0 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -31,6 +31,7 @@ #include +#include #include #include #include @@ -42,122 +43,6 @@ namespace panther_lights using std::placeholders::_1; using std::placeholders::_2; -LEDAnimation::LEDAnimation( - const LEDAnimationDescription & led_animation_description, - const std::unordered_map> & segments, - const rclcpp::Time & init_time) -: led_animation_description_(led_animation_description), - init_time_(init_time), - repeating_(false), - param_("") -{ - for (auto & animation : led_animation_description_.animations) { - for (auto & segment : animation.segments) { - animation_segments_.push_back(segments.at(segment)); - } - } -} - -bool LEDAnimation::IsFinished() -{ - return std::all_of( - animation_segments_.begin(), animation_segments_.end(), - [](const std::shared_ptr & segment) { return segment->IsAnimationFinished(); }); -} - -float LEDAnimation::GetProgress() const -{ - float progress = 1.0f; - std::for_each( - animation_segments_.begin(), animation_segments_.end(), - [&progress](const std::shared_ptr & segment) { - auto anim_progress = segment->GetAnimationProgress(); - progress = anim_progress < progress ? anim_progress : progress; - }); - - return progress; -} - -void LEDAnimation::Reset() const -{ - std::for_each( - animation_segments_.begin(), animation_segments_.end(), - [](const std::shared_ptr & segment) { segment->ResetAnimation(); }); -} - -void AnimationsQueue::Put( - const std::shared_ptr & animation, const rclcpp::Time & time) -{ - if (animation->GetPriority() == 1) { - Clear(); - } - Validate(time); - - if (queue_.size() == max_queue_size_) { - throw std::runtime_error("Animation queue overloaded"); - } - - queue_.push_back(animation); - std::sort( - queue_.begin(), queue_.end(), - [](const std::shared_ptr & a, const std::shared_ptr & b) { - return a->GetPriority() < b->GetPriority() || - (a->GetPriority() == b->GetPriority() && a->GetInitTime() < b->GetInitTime()); - }); -} - -std::shared_ptr AnimationsQueue::Get() -{ - if (!queue_.empty()) { - const auto animation = queue_.front(); - queue_.pop_front(); - return animation; - } - throw std::runtime_error("Queue empty"); -} - -void AnimationsQueue::Clear(std::size_t priority) -{ - const auto new_end = std::remove_if( - queue_.begin(), queue_.end(), [priority](const std::shared_ptr & animation) { - return animation->GetPriority() >= priority; - }); - queue_.erase(new_end, queue_.end()); -} - -void AnimationsQueue::Remove(const std::shared_ptr & animation) -{ - auto it = std::find(queue_.begin(), queue_.end(), animation); - if (it != queue_.end()) { - queue_.erase(it); - } -} - -bool AnimationsQueue::HasAnimation(const std::shared_ptr & animation) const -{ - return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); -} - -void AnimationsQueue::Validate(const rclcpp::Time & time) -{ - for (auto it = queue_.begin(); it != queue_.end();) { - if ((time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { - // TODO: log info - animation timeout - it = queue_.erase(it); - } else { - ++it; - } - } -} - -std::size_t AnimationsQueue::GetFirstAnimationPriority() const -{ - if (!Empty()) { - return queue_.front()->GetPriority(); - } - return LEDAnimation::kDefaultPriority; -} - ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { @@ -178,7 +63,7 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node segment_converter_ = std::make_shared(); - animation_queue_ = std::make_unique(10); + animations_queue_ = std::make_unique(10); set_led_animation_server_ = this->create_service( "lights/controller/set/animation", std::bind(&ControllerNode::SetLEDAnimationCB, this, _1, _2)); @@ -347,11 +232,11 @@ void ControllerNode::ControllerTimerCB() auto brightness = 255; if (animation_finished_) { - animation_queue_->Validate(this->get_clock()->now()); + animations_queue_->Validate(this->get_clock()->now()); - if (!animation_queue_->Empty()) { + if (!animations_queue_->Empty()) { try { - SetLEDAnimation(animation_queue_->Get()); + SetLEDAnimation(animations_queue_->Get()); } catch (const std::runtime_error & e) { RCLCPP_ERROR(this->get_logger(), "Failed to Set LED animation: %s", e.what()); } @@ -364,10 +249,10 @@ void ControllerNode::ControllerTimerCB() return; } - if (current_animation_->GetPriority() > animation_queue_->GetFirstAnimationPriority()) { + if (current_animation_->GetPriority() > animations_queue_->GetFirstAnimationPriority()) { if (current_animation_->GetProgress() < 0.65f) { current_animation_->Reset(); - animation_queue_->Put(current_animation_, this->get_clock()->now()); + animations_queue_->Put(current_animation_, this->get_clock()->now()); } animation_finished_ = true; return; @@ -415,8 +300,8 @@ void ControllerNode::AddAnimationToQueue(const std::size_t animation_id, const b animation_description, segments_, this->get_clock()->now()); animation->SetRepeating(repeating); // animation->SetParam(param); - animation_queue_->Put(animation, this->get_clock()->now()); - animation_queue_->Print(); + animations_queue_->Put(animation, this->get_clock()->now()); + animations_queue_->Print(); } void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_animation) diff --git a/panther_lights/src/led_animations_queue.cpp b/panther_lights/src/led_animations_queue.cpp new file mode 100644 index 000000000..63e3c88c5 --- /dev/null +++ b/panther_lights/src/led_animations_queue.cpp @@ -0,0 +1,147 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include + +namespace panther_lights +{ + +using std::placeholders::_1; +using std::placeholders::_2; + +LEDAnimation::LEDAnimation( + const LEDAnimationDescription & led_animation_description, + const std::unordered_map> & segments, + const rclcpp::Time & init_time) +: led_animation_description_(led_animation_description), + init_time_(init_time), + repeating_(false), + param_("") +{ + for (auto & animation : led_animation_description_.animations) { + for (auto & segment : animation.segments) { + animation_segments_.push_back(segments.at(segment)); + } + } +} + +bool LEDAnimation::IsFinished() +{ + return std::all_of( + animation_segments_.begin(), animation_segments_.end(), + [](const std::shared_ptr & segment) { return segment->IsAnimationFinished(); }); +} + +float LEDAnimation::GetProgress() const +{ + float progress = 1.0f; + std::for_each( + animation_segments_.begin(), animation_segments_.end(), + [&progress](const std::shared_ptr & segment) { + auto anim_progress = segment->GetAnimationProgress(); + progress = anim_progress < progress ? anim_progress : progress; + }); + + return progress; +} + +void LEDAnimation::Reset() const +{ + std::for_each( + animation_segments_.begin(), animation_segments_.end(), + [](const std::shared_ptr & segment) { segment->ResetAnimation(); }); +} + +void LEDAnimationsQueue::Put( + const std::shared_ptr & animation, const rclcpp::Time & time) +{ + if (animation->GetPriority() == 1) { + Clear(); + } + Validate(time); + + if (queue_.size() == max_queue_size_) { + throw std::runtime_error("Animation queue overloaded"); + } + + queue_.push_back(animation); + std::sort( + queue_.begin(), queue_.end(), + [](const std::shared_ptr & a, const std::shared_ptr & b) { + return a->GetPriority() < b->GetPriority() || + (a->GetPriority() == b->GetPriority() && a->GetInitTime() < b->GetInitTime()); + }); +} + +std::shared_ptr LEDAnimationsQueue::Get() +{ + if (!queue_.empty()) { + const auto animation = queue_.front(); + queue_.pop_front(); + return animation; + } + throw std::runtime_error("Queue empty"); +} + +void LEDAnimationsQueue::Clear(std::size_t priority) +{ + const auto new_end = std::remove_if( + queue_.begin(), queue_.end(), [priority](const std::shared_ptr & animation) { + return animation->GetPriority() >= priority; + }); + queue_.erase(new_end, queue_.end()); +} + +void LEDAnimationsQueue::Remove(const std::shared_ptr & animation) +{ + auto it = std::find(queue_.begin(), queue_.end(), animation); + if (it != queue_.end()) { + queue_.erase(it); + } +} + +bool LEDAnimationsQueue::HasAnimation(const std::shared_ptr & animation) const +{ + return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); +} + +void LEDAnimationsQueue::Validate(const rclcpp::Time & time) +{ + for (auto it = queue_.begin(); it != queue_.end();) { + if ((time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { + // TODO: log info - animation timeout + it = queue_.erase(it); + } else { + ++it; + } + } +} + +std::size_t LEDAnimationsQueue::GetFirstAnimationPriority() const +{ + if (!Empty()) { + return queue_.front()->GetPriority(); + } + return LEDAnimation::kDefaultPriority; +} + +} // namespace panther_lights From 511661a84401692c17a20f2bb9a4dd7dae4093fe Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 26 Feb 2024 11:22:44 +0000 Subject: [PATCH 11/32] add briefs --- .../panther_lights/led_animations_queue.hpp | 78 +++++++++++++++++-- panther_lights/src/controller_node.cpp | 2 +- panther_lights/src/led_animations_queue.cpp | 29 +++---- 3 files changed, 87 insertions(+), 22 deletions(-) diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp index 1c138a542..af9b0423b 100644 --- a/panther_lights/include/panther_lights/led_animations_queue.hpp +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -47,9 +47,19 @@ struct LEDAnimationDescription std::vector animations; }; +/** + * @brief Class representing animation that can be displayed on robot segments + */ class LEDAnimation { public: + /** + * @brief Initializes LED animaiton + * + * @param led_animation_description YAML description of the LED animation + * @param segments This parameter is used to create map of segments used by this LED animation + * @param init_time Time of creation of the LED animation + */ LEDAnimation( const LEDAnimationDescription & led_animation_description, const std::unordered_map> & segments, @@ -57,9 +67,19 @@ class LEDAnimation ~LEDAnimation() {} + /** + * @brief Indicates if LED animation is finished + * + * @return True if all animations on all segments are finished, false otherwise + */ bool IsFinished(); - void ResetTime(const rclcpp::Time & init_time) { init_time_ = init_time; } + /** + * @brief Reset all animaitons on all LED segments + * + * @param time This time is used to set new animation initialization time + */ + void Reset(const rclcpp::Time & time); std::string GetName() const { return led_animation_description_.name; } std::uint8_t GetPriority() const { return led_animation_description_.priority; } @@ -69,13 +89,15 @@ class LEDAnimation } rclcpp::Time GetInitTime() const { return init_time_; } float GetTimeout() const { return led_animation_description_.timeout; } - LEDAnimationDescription GetAnimationDescription() const { return led_animation_description_; } + + /** + * @brief Get LED animation progress + * + * @return The smallest progress of all animations on all segments + */ float GetProgress() const; - void Reset() const; bool IsRepeating() const { return repeating_; } - - void SetInitTime(const rclcpp::Time init_time) { init_time_ = init_time; } void SetRepeating(const bool value) { repeating_ = value; } static constexpr char kDefaultName[] = "UNDEFINED"; @@ -91,21 +113,63 @@ class LEDAnimation std::vector> animation_segments_; }; +/** + * @brief Class used to manage queue of LED animations + */ class LEDAnimationsQueue { public: + /** + * @brief Initializes LED animaitons queue + * + * @param max_queue_size Max size of the queue + */ LEDAnimationsQueue(const std::size_t max_queue_size = 5) : max_queue_size_(max_queue_size) {} + /** + * @brief Add animation to the queue and sort animations according to their priority and time of + * initialization + * + * @param animation LED animation to add to queue + * @param time Time of initialization of the animation + * + * @exception std::runtime_error if queue has number of elements equal to max_queue_size + */ void Put(const std::shared_ptr & animation, const rclcpp::Time & time); + + /** + * @brief Get and remove first LED animation from the queue + * + * @return First LED animation from the queue + * + * @exception std::runtime_error if queue is empty + */ std::shared_ptr Get(); + + /** + * @brief Remove all animations with priority equal or lower to specified one + * + * @param priority Animation with this priority or lower will be removed from the queue + */ void Clear(std::size_t priority = 2); - void Remove(const std::shared_ptr & animation); + /** + * @brief Removes animations that has reached theirs timeout from the queue + * + * @param time Time used to check if animation has timed out + */ void Validate(const rclcpp::Time & time); - bool HasAnimation(const std::shared_ptr & animation) const; + /** + * @brief Return priority of the first animation in the queue + * + * @return Priority of the first animation or default animation priority if queue is empty + */ std::size_t GetFirstAnimationPriority() const; + void Remove(const std::shared_ptr & animation); + bool HasAnimation(const std::shared_ptr & animation) const; + bool Empty() const { return queue_.empty(); } void Print() diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 4026898d0..b7d0514b5 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -251,7 +251,7 @@ void ControllerNode::ControllerTimerCB() if (current_animation_->GetPriority() > animations_queue_->GetFirstAnimationPriority()) { if (current_animation_->GetProgress() < 0.65f) { - current_animation_->Reset(); + current_animation_->Reset(this->get_clock()->now()); animations_queue_->Put(current_animation_, this->get_clock()->now()); } animation_finished_ = true; diff --git a/panther_lights/src/led_animations_queue.cpp b/panther_lights/src/led_animations_queue.cpp index 63e3c88c5..305386785 100644 --- a/panther_lights/src/led_animations_queue.cpp +++ b/panther_lights/src/led_animations_queue.cpp @@ -64,11 +64,12 @@ float LEDAnimation::GetProgress() const return progress; } -void LEDAnimation::Reset() const +void LEDAnimation::Reset(const rclcpp::Time & time) { std::for_each( animation_segments_.begin(), animation_segments_.end(), [](const std::shared_ptr & segment) { segment->ResetAnimation(); }); + init_time_ = time; } void LEDAnimationsQueue::Put( @@ -111,19 +112,6 @@ void LEDAnimationsQueue::Clear(std::size_t priority) queue_.erase(new_end, queue_.end()); } -void LEDAnimationsQueue::Remove(const std::shared_ptr & animation) -{ - auto it = std::find(queue_.begin(), queue_.end(), animation); - if (it != queue_.end()) { - queue_.erase(it); - } -} - -bool LEDAnimationsQueue::HasAnimation(const std::shared_ptr & animation) const -{ - return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); -} - void LEDAnimationsQueue::Validate(const rclcpp::Time & time) { for (auto it = queue_.begin(); it != queue_.end();) { @@ -144,4 +132,17 @@ std::size_t LEDAnimationsQueue::GetFirstAnimationPriority() const return LEDAnimation::kDefaultPriority; } +void LEDAnimationsQueue::Remove(const std::shared_ptr & animation) +{ + auto it = std::find(queue_.begin(), queue_.end(), animation); + if (it != queue_.end()) { + queue_.erase(it); + } +} + +bool LEDAnimationsQueue::HasAnimation(const std::shared_ptr & animation) const +{ + return std::find(queue_.begin(), queue_.end(), animation) != queue_.end(); +} + } // namespace panther_lights From 324f5855f91c9d90168e51013d6bbe538ec282d5 Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 26 Feb 2024 12:57:16 +0000 Subject: [PATCH 12/32] param and brightness handle --- .../include/panther_lights/controller_node.hpp | 3 ++- .../panther_lights/led_animations_queue.hpp | 3 +++ .../include/panther_lights/led_segment.hpp | 12 ++++++++++-- panther_lights/src/controller_node.cpp | 11 ++++++----- panther_lights/src/led_segment.cpp | 16 +++++++++++++--- panther_lights/src/segment_converter.cpp | 8 +++++++- 6 files changed, 41 insertions(+), 12 deletions(-) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 1726e54bf..96296bb38 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -101,7 +101,8 @@ class ControllerNode : public rclcpp::Node void SetAnimation(const std::size_t animation_id, const bool repeating); void UpdateAndPublishAnimation(); - void AddAnimationToQueue(const std::size_t animation_id, const bool repeating); + void AddAnimationToQueue( + const std::size_t animation_id, const bool repeating, const std::string & param); void SetLEDAnimation(const std::shared_ptr & led_animation); void PublishPanelFrame(const std::size_t channel); diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp index af9b0423b..317a472a2 100644 --- a/panther_lights/include/panther_lights/led_animations_queue.hpp +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -98,7 +98,10 @@ class LEDAnimation float GetProgress() const; bool IsRepeating() const { return repeating_; } + std::string GetParam() const { return param_; } + void SetRepeating(const bool value) { repeating_ = value; } + void SetParam(const std::string & param) { param_ = param; } static constexpr char kDefaultName[] = "UNDEFINED"; static constexpr std::uint8_t kDefaultPriority = 3; diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 94f54eb3a..ab4208a56 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -59,7 +59,8 @@ class LEDSegment * animation fails to initialize */ void SetAnimation( - const std::string & type, const YAML::Node & animation_description, const bool repeating); + const std::string & type, const YAML::Node & animation_description, const bool repeating, + const std::string & param = ""); /** * @brief Update animation frame @@ -68,7 +69,7 @@ class LEDSegment * * @exception std::runtime_error if fails to update animation */ - void UpdateAnimation(const std::string & param = ""); + void UpdateAnimation(); /** * @brief Check if animation is finished. This does not return state of the default animation @@ -102,6 +103,13 @@ class LEDSegment */ void ResetAnimation() const; + /** + * @brief Get current animation brightness + * + * @exception std::runtime_error if segment animation is not defined + */ + std::uint8_t GetAnimationBrightness() const; + std::size_t GetFirstLEDPosition() const; std::size_t GetChannel() const { return channel_; } diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index b7d0514b5..470cdb5b3 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -199,8 +199,7 @@ void ControllerNode::SetLEDAnimationCB( SetLEDAnimationSrv::Response::SharedPtr response) { try { - // request->animation.param; TODO - AddAnimationToQueue(request->animation.id, request->repeating); + AddAnimationToQueue(request->animation.id, request->repeating, request->animation.param); response->success = true; } catch (const std::exception & e) { response->success = false; @@ -289,7 +288,8 @@ void ControllerNode::UpdateAndPublishAnimation() } } -void ControllerNode::AddAnimationToQueue(const std::size_t animation_id, const bool repeating) +void ControllerNode::AddAnimationToQueue( + const std::size_t animation_id, const bool repeating, const std::string & param) { if (animations_descriptions_.find(animation_id) == animations_descriptions_.end()) { throw std::runtime_error("No animation with ID: " + std::to_string(animation_id)); @@ -299,7 +299,7 @@ void ControllerNode::AddAnimationToQueue(const std::size_t animation_id, const b auto animation = std::make_shared( animation_description, segments_, this->get_clock()->now()); animation->SetRepeating(repeating); - // animation->SetParam(param); + animation->SetParam(param); animations_queue_->Put(animation, this->get_clock()->now()); animations_queue_->Print(); } @@ -315,7 +315,8 @@ void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_a try { segments_.at(segment)->SetAnimation( - animation.type, animation.animation, led_animation->IsRepeating()); + animation.type, animation.animation, led_animation->IsRepeating(), + led_animation->GetParam()); } catch (const std::runtime_error & e) { throw std::runtime_error( "Failed to set '" + led_animation->GetName() + "' animation: " + std::string(e.what())); diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index 00eab7336..c53bdb200 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -75,7 +75,8 @@ LEDSegment::~LEDSegment() } void LEDSegment::SetAnimation( - const std::string & type, const YAML::Node & animation_description, const bool repeating) + const std::string & type, const YAML::Node & animation_description, const bool repeating, + const std::string & param) { std::shared_ptr animation; @@ -87,6 +88,7 @@ void LEDSegment::SetAnimation( try { animation->Initialize(animation_description, num_led_, controller_frequency_); + animation->SetParam(param); } catch (const std::runtime_error & e) { throw std::runtime_error("Failed to initialize animation: " + std::string(e.what())); } catch (const std::out_of_range & e) { @@ -104,7 +106,7 @@ void LEDSegment::SetAnimation( } } -void LEDSegment::UpdateAnimation(const std::string & param) +void LEDSegment::UpdateAnimation() { if (!animation_) { throw std::runtime_error("Segment animation not defined"); @@ -122,7 +124,6 @@ void LEDSegment::UpdateAnimation(const std::string & param) } try { - animation_to_update->SetParam(param); animation_to_update->Update(); } catch (const std::runtime_error & e) { throw std::runtime_error("Failed to update animation: " + std::string(e.what())); @@ -160,6 +161,15 @@ void LEDSegment::ResetAnimation() const animation_->Reset(); } +std::uint8_t LEDSegment::GetAnimationBrightness() const +{ + if (!animation_) { + throw std::runtime_error("Segment animation not defined"); + } + + animation_->GetBrightness(); +} + std::size_t LEDSegment::GetFirstLEDPosition() const { return (invert_led_order_ ? last_led_iterator_ : first_led_iterator_) * Animation::kRGBAColorLen; diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp index ea16f346b..84b624735 100644 --- a/panther_lights/src/segment_converter.cpp +++ b/panther_lights/src/segment_converter.cpp @@ -33,7 +33,13 @@ void SegmentConverter::Convert( for (auto & [segment_name, segment] : segments) { try { auto panel = panels.at(segment->GetChannel()); - panel->UpdateFrame(segment->GetFirstLEDPosition(), segment->GetAnimationFrame()); + + auto frame = segment->GetAnimationFrame(); + for (std::size_t i = 3; i < frame.size(); i += 4) { + frame[i] = segment->GetAnimationBrightness(); + } + + panel->UpdateFrame(segment->GetFirstLEDPosition(), frame); } catch (const std::runtime_error & e) { throw std::runtime_error( "Failed to convert '" + segment_name + From 276a70d147e69aecc9b0c1a09933190904a6b156 Mon Sep 17 00:00:00 2001 From: Dawid Date: Mon, 26 Feb 2024 14:41:12 +0000 Subject: [PATCH 13/32] user animations, bugs, briefs --- .../panther_lights/controller_node.hpp | 45 +++++- .../panther_lights/led_animations_queue.hpp | 4 +- .../include/panther_lights/led_segment.hpp | 2 + panther_lights/src/controller_node.cpp | 131 ++++++++++++------ panther_lights/src/segment_converter.cpp | 5 + 5 files changed, 143 insertions(+), 44 deletions(-) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 96296bb38..c5ec3fa35 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -82,14 +82,31 @@ class ControllerNode : public rclcpp::Node void InitializeLEDSegmentsMap(const YAML::Node & segments_map_description); /** - * @brief Adds animations from YAML description to an unordered map with animations + * @brief Adds animations to an unordered map with animations * - * @param animations_description YAML description with list of animations to be loaded + * @param animations_description YAML description with list of animations * * @exception std::runtime_error if fails to load an animation or animation with given ID already * exists in the map */ - void LoadAnimations(const YAML::Node & animations_description); + void LoadDefaultAnimations(const YAML::Node & animations_description); + + /** + * @brief Adds animations to an unordered map with animations + * + * @param user_led_animations_file path to YAML file with user animations description + */ + void LoadUserAnimations(const std::string & user_led_animations_file); + + /** + * @brief Adds animation to an unordered map with animations + * + * @param animations_description YAML description of the animation + * + * @exception std::runtime_error if fails to load an animation or animation with given ID already + * exists in the map + */ + void LoadAnimation(const YAML::Node & animation_description); /** * @brief Sets animation with requested ID @@ -100,9 +117,31 @@ class ControllerNode : public rclcpp::Node */ void SetAnimation(const std::size_t animation_id, const bool repeating); + /** + * @brief Updates all segments animations, converts then into panel frames and publishes panel + * frames on respective topics + */ void UpdateAndPublishAnimation(); + + /** + * @brief Add animation to LED animations queue + * + * @param animation_id ID of the animations + * @param repeating Whether animations should repeat + * @param param Optional animaiton parameter + * + * @exception std::runtime_error if no animation with given ID exists + */ void AddAnimationToQueue( const std::size_t animation_id, const bool repeating, const std::string & param); + + /** + * @brief Add animations to LED segments based on LED animation + * + * @param led_animation LED animation + * + * @exception std::runtime_error animation has invalid segment name or it fails to load + */ void SetLEDAnimation(const std::shared_ptr & led_animation); void PublishPanelFrame(const std::size_t channel); diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp index 317a472a2..08f9f6fbf 100644 --- a/panther_lights/include/panther_lights/led_animations_queue.hpp +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -15,6 +15,7 @@ #ifndef PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ #define PANTHER_LIGHTS_LED_ANIMATIONS_QUEUE_HPP_ +#include #include #include #include @@ -40,7 +41,7 @@ struct AnimationDescription struct LEDAnimationDescription { - std::size_t id; + std::uint8_t id; std::uint8_t priority; std::string name; float timeout; @@ -106,6 +107,7 @@ class LEDAnimation static constexpr char kDefaultName[] = "UNDEFINED"; static constexpr std::uint8_t kDefaultPriority = 3; static constexpr float kDefaultTimeout = 120.0f; + static constexpr std::array kValidPriorities = {1, 2, 3}; private: const LEDAnimationDescription led_animation_description_; diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index ab4208a56..3019149ea 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -114,6 +114,8 @@ class LEDSegment std::size_t GetChannel() const { return channel_; } + bool HasAnimation() const { return animation_ || default_animation_; } + private: const float controller_frequency_; bool invert_led_order_ = false; diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 470cdb5b3..fedb5ffdb 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -49,9 +49,11 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node this->declare_parameter( "led_config_file", "/home/husarion/ros2_ws/src/panther_ros/panther_lights/config/led_config.yaml"); + this->declare_parameter("user_led_animaitons_file", ""); this->declare_parameter("controller_freq", 50.0); const auto led_config_file = this->get_parameter("led_config_file").as_string(); + const auto user_led_animations_file = this->get_parameter("user_led_animaitons_file").as_string(); const float controller_freq = this->get_parameter("controller_freq").as_double(); YAML::Node led_config_desc = YAML::LoadFile(led_config_file); @@ -59,7 +61,11 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node InitializeLEDPanels(led_config_desc["panels"]); InitializeLEDSegments(led_config_desc["segments"], controller_freq); InitializeLEDSegmentsMap(led_config_desc["segments_map"]); - LoadAnimations(led_config_desc["animations"]); + LoadDefaultAnimations(led_config_desc["animations"]); + + if (user_led_animations_file != "") { + LoadUserAnimations(user_led_animations_file); + } segment_converter_ = std::make_shared(); @@ -149,49 +155,95 @@ void ControllerNode::InitializeLEDSegmentsMap(const YAML::Node & segments_map_de } } -void ControllerNode::LoadAnimations(const YAML::Node & animations_description) +void ControllerNode::LoadDefaultAnimations(const YAML::Node & animations_description) { - RCLCPP_INFO(this->get_logger(), "Loading LED animations"); + RCLCPP_INFO(this->get_logger(), "Loading users LED animations"); + for (auto & animation_description : animations_description.as>()) { - LEDAnimationDescription led_animation_desc; + LoadAnimation(animation_description); + } - try { - led_animation_desc.name = panther_utils::GetYAMLKeyValue( - animation_description, "name", LEDAnimation::kDefaultName); - led_animation_desc.id = panther_utils::GetYAMLKeyValue( - animation_description, "id"); - led_animation_desc.priority = panther_utils::GetYAMLKeyValue( - animation_description, "priority", LEDAnimation::kDefaultPriority); - led_animation_desc.timeout = panther_utils::GetYAMLKeyValue( - animation_description, "timeout", LEDAnimation::kDefaultTimeout); - - auto animations = panther_utils::GetYAMLKeyValue>( - animation_description, "animations"); - for (auto & animation : animations) { - AnimationDescription animation_desc; - animation_desc.type = panther_utils::GetYAMLKeyValue(animation, "type"); - animation_desc.animation = panther_utils::GetYAMLKeyValue( - animation, "animation"); - - auto segments_group = panther_utils::GetYAMLKeyValue(animation, "segments"); - animation_desc.segments = segments_map_.at(segments_group); - - led_animation_desc.animations.push_back(animation_desc); - } + RCLCPP_INFO(this->get_logger(), "Animations successfully loaded"); +} - const auto result = animations_descriptions_.emplace( - led_animation_desc.id, led_animation_desc); - if (!result.second) { - throw std::runtime_error("Animation with given ID already exists."); - } +void ControllerNode::LoadUserAnimations(const std::string & user_led_animations_file) +{ + RCLCPP_INFO(this->get_logger(), "Loading users LED animations"); - } catch (const std::runtime_error & e) { - throw std::runtime_error( - "Failed to load '" + led_animation_desc.name + "' animation: " + std::string(e.what())); + try { + YAML::Node user_led_animaitons = YAML::LoadFile(user_led_animations_file); + auto user_animations = panther_utils::GetYAMLKeyValue>( + user_led_animaitons, "user_animations"); + + for (auto & animation_description : user_animations) { + try { + auto id = panther_utils::GetYAMLKeyValue(animation_description, "id"); + if (id < 20) { + throw std::runtime_error("Animation ID must be greater than 19"); + } + + auto priority = panther_utils::GetYAMLKeyValue( + animation_description, "priority", LEDAnimation::kDefaultPriority); + if (priority == 1) { + throw std::runtime_error("User animation can not have priority 1"); + } + + LoadAnimation(animation_description); + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + this->get_logger(), "Skipping user animation that failed to load: %s", e.what()); + } } + } catch (const std::exception & e) { + RCLCPP_WARN(this->get_logger(), "Failed to load user animations: %s", e.what()); } - RCLCPP_INFO(this->get_logger(), "Animations Successfully loaded"); + RCLCPP_INFO(this->get_logger(), "User animations successfully loaded"); +} + +void ControllerNode::LoadAnimation(const YAML::Node & animation_description) +{ + LEDAnimationDescription led_animation_desc; + + try { + led_animation_desc.name = panther_utils::GetYAMLKeyValue( + animation_description, "name", LEDAnimation::kDefaultName); + led_animation_desc.id = panther_utils::GetYAMLKeyValue( + animation_description, "id"); + led_animation_desc.priority = panther_utils::GetYAMLKeyValue( + animation_description, "priority", LEDAnimation::kDefaultPriority); + led_animation_desc.timeout = panther_utils::GetYAMLKeyValue( + animation_description, "timeout", LEDAnimation::kDefaultTimeout); + + if ( + std::find( + LEDAnimation::kValidPriorities.begin(), LEDAnimation::kValidPriorities.end(), + led_animation_desc.priority) == LEDAnimation::kValidPriorities.end()) { + throw std::runtime_error("Invalid LED animation ID"); + } + + auto animations = panther_utils::GetYAMLKeyValue>( + animation_description, "animations"); + for (auto & animation : animations) { + AnimationDescription animation_desc; + animation_desc.type = panther_utils::GetYAMLKeyValue(animation, "type"); + animation_desc.animation = panther_utils::GetYAMLKeyValue(animation, "animation"); + + auto segments_group = panther_utils::GetYAMLKeyValue(animation, "segments"); + animation_desc.segments = segments_map_.at(segments_group); + + led_animation_desc.animations.push_back(animation_desc); + } + + const auto result = animations_descriptions_.emplace(led_animation_desc.id, led_animation_desc); + if (!result.second) { + throw std::runtime_error("Animation with given ID already exists."); + } + + } catch (const std::runtime_error & e) { + throw std::runtime_error( + "Failed to load '" + led_animation_desc.name + "' animation: " + std::string(e.what())); + } } void ControllerNode::SetLEDAnimationCB( @@ -228,8 +280,6 @@ void ControllerNode::ControllerTimerCB() { std::lock_guard lck_g(queue_mtx_); - auto brightness = 255; - if (animation_finished_) { animations_queue_->Validate(this->get_clock()->now()); @@ -242,7 +292,6 @@ void ControllerNode::ControllerTimerCB() } } - // if(curent_animation) // optional if (!current_animation_) { RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Waiting for animation"); return; @@ -268,7 +317,9 @@ void ControllerNode::UpdateAndPublishAnimation() for (auto & [segment_name, segment] : segments_) { try { - segment->UpdateAnimation(); + if (segment->HasAnimation()) { + segment->UpdateAnimation(); + } } catch (const std::runtime_error & e) { RCLCPP_WARN( this->get_logger(), "Failed to update animation on %s segment: %s", segment_name.c_str(), diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp index 84b624735..22a600329 100644 --- a/panther_lights/src/segment_converter.cpp +++ b/panther_lights/src/segment_converter.cpp @@ -31,6 +31,11 @@ void SegmentConverter::Convert( const std::unordered_map> & panels) { for (auto & [segment_name, segment] : segments) { + if (!segment->HasAnimation()) { + continue; + ; + } + try { auto panel = panels.at(segment->GetChannel()); From beb7ae5fcb9b6cfd6bfce2485323e40e14f5ca05 Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 27 Feb 2024 08:56:48 +0000 Subject: [PATCH 14/32] use yaml utils --- panther_lights/CMakeLists.txt | 12 ++++++----- .../panther_lights/animation/animation.hpp | 14 +++++-------- .../src/animation/image_animation.cpp | 9 ++++---- panther_lights/src/led_segment.cpp | 21 ++++++------------- 4 files changed, 22 insertions(+), 34 deletions(-) diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 3791d8ab9..4fc2652e5 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -23,7 +23,7 @@ pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) add_library( panther_animation_plugins SHARED src/animation/image_animation.cpp src/animation/charging_animation.cpp) -ament_target_dependencies(panther_animation_plugins pluginlib) +ament_target_dependencies(panther_animation_plugins panther_utils pluginlib) target_link_libraries(panther_animation_plugins png yaml-cpp) add_executable(driver_node src/driver_node_main.cpp src/driver_node.cpp @@ -51,7 +51,7 @@ add_executable(dummy_scheduler_node src/dummy_scheduler_node_main.cpp src/dummy_scheduler_node.cpp) ament_target_dependencies(dummy_scheduler_node image_transport rclcpp - sensor_msgs) + panther_msgs sensor_msgs) install(TARGETS driver_node controller_node dummy_scheduler_node DESTINATION lib/${PROJECT_NAME}) @@ -73,6 +73,7 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_animation PUBLIC $ $) + ament_target_dependencies(${PROJECT_NAME}_test_animation panther_utils) target_link_libraries(${PROJECT_NAME}_test_animation yaml-cpp) ament_add_gtest( @@ -83,7 +84,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_image_animation - ament_index_cpp pluginlib) + ament_index_cpp panther_utils pluginlib) target_link_libraries(${PROJECT_NAME}_test_image_animation png yaml-cpp) ament_add_gtest( @@ -94,7 +95,7 @@ if(BUILD_TESTING) PUBLIC $ $) ament_target_dependencies(${PROJECT_NAME}_test_charging_animation - ament_index_cpp pluginlib) + ament_index_cpp panther_utils pluginlib) target_link_libraries(${PROJECT_NAME}_test_charging_animation yaml-cpp) ament_add_gtest(${PROJECT_NAME}_test_led_panel test/test_led_panel.cpp @@ -121,7 +122,8 @@ if(BUILD_TESTING) ${PROJECT_NAME}_test_segment_converter PUBLIC $ $) - ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib) + ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib + panther_utils) target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp) endif() diff --git a/panther_lights/include/panther_lights/animation/animation.hpp b/panther_lights/include/panther_lights/animation/animation.hpp index 6546e95f5..e81eaca00 100644 --- a/panther_lights/include/panther_lights/animation/animation.hpp +++ b/panther_lights/include/panther_lights/animation/animation.hpp @@ -25,6 +25,8 @@ #include +#include + namespace panther_lights { @@ -51,18 +53,12 @@ class Animation num_led_ = num_led; frame_ = std::vector(num_led_ * kRGBAColorLen, 0); - if (!animation_description["duration"]) { - throw std::runtime_error("Missing 'duration' in animation description"); - } - - auto duration = animation_description["duration"].as(); + auto duration = panther_utils::GetYAMLKeyValue(animation_description, "duration"); if ((duration - std::numeric_limits::epsilon()) <= 0.0) { throw std::out_of_range("Duration has to be positive"); } - if (animation_description["repeat"]) { - loops_ = animation_description["repeat"].as(); - } + loops_ = panther_utils::GetYAMLKeyValue(animation_description, "repeat", 1); if (duration * loops_ > 10.0) { throw std::runtime_error("Animation display duration (duration * repeat) exceeds 10 seconds"); @@ -182,7 +178,7 @@ class Animation bool finished_ = false; float progress_ = 0.0; - std::size_t loops_ = 1; + std::size_t loops_; std::size_t current_cycle_ = 0; std::size_t anim_iteration_ = 0; std::uint8_t brightness_ = 255; diff --git a/panther_lights/src/animation/image_animation.cpp b/panther_lights/src/animation/image_animation.cpp index 2fb930cf3..b8463dd79 100644 --- a/panther_lights/src/animation/image_animation.cpp +++ b/panther_lights/src/animation/image_animation.cpp @@ -30,6 +30,8 @@ #include #include +#include + namespace panther_lights { @@ -39,11 +41,8 @@ void ImageAnimation::Initialize( { Animation::Initialize(animation_description, num_led, controller_frequency); - if (!animation_description["image"]) { - throw std::runtime_error("No 'image' in animation description"); - } - - const auto image_path = ParseImagePath(animation_description["image"].as()); + const auto image_path = + ParseImagePath(panther_utils::GetYAMLKeyValue(animation_description, "image")); gil::rgba8_image_t base_image; gil::read_and_convert_image(std::string(image_path), base_image, gil::png_tag()); image_ = RGBAImageResize(base_image, this->GetNumberOfLeds(), this->GetAnimationLength()); diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index c53bdb200..7741f837e 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace panther_lights { @@ -28,21 +29,10 @@ namespace panther_lights LEDSegment::LEDSegment(const YAML::Node & segment_description, const float controller_frequency) : controller_frequency_(controller_frequency) { - if (!segment_description["led_range"]) { - throw std::runtime_error("Missing 'led_range' in segment description"); - } - - if (!segment_description["channel"]) { - throw std::runtime_error("Missing 'channel' in segment description"); - } + channel_ = panther_utils::GetYAMLKeyValue(segment_description, "channel"); + const auto led_range = panther_utils::GetYAMLKeyValue( + segment_description, "led_range"); - try { - channel_ = segment_description["channel"].as(); - } catch (const YAML::BadConversion & e) { - throw std::invalid_argument("Invalid channel expression: " + std::string(e.what())); - } - - const auto led_range = segment_description["led_range"].as(); const std::size_t split_char = led_range.find('-'); if (split_char == std::string::npos) { @@ -100,6 +90,7 @@ void LEDSegment::SetAnimation( if (repeating) { default_animation_ = animation_; + animation_finished_ = true; } if (default_animation_) { default_animation_->Reset(); @@ -167,7 +158,7 @@ std::uint8_t LEDSegment::GetAnimationBrightness() const throw std::runtime_error("Segment animation not defined"); } - animation_->GetBrightness(); + return animation_->GetBrightness(); } std::size_t LEDSegment::GetFirstLEDPosition() const From 0449be55bfae8bddb29ddb50ee13a40add228f35 Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 27 Feb 2024 08:57:04 +0000 Subject: [PATCH 15/32] fix tests --- panther_lights/test/test_animation.cpp | 6 +- panther_lights/test/test_led_segment.cpp | 50 ++++++-------- .../test/test_segment_converter.cpp | 68 +++++++++++-------- 3 files changed, 66 insertions(+), 58 deletions(-) diff --git a/panther_lights/test/test_animation.cpp b/panther_lights/test/test_animation.cpp index 331565ea7..9f778a05c 100644 --- a/panther_lights/test/test_animation.cpp +++ b/panther_lights/test/test_animation.cpp @@ -74,7 +74,7 @@ TEST_F(TestAnimation, Initialize) animation_description["duration"] = "-1.0"; EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::out_of_range); animation_description["duration"] = "word"; - EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), YAML::BadConversion); + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); // invalid animation length animation_description["duration"] = "0.1"; @@ -85,9 +85,9 @@ TEST_F(TestAnimation, Initialize) // invalid repeat animation_description["repeat"] = "-2"; - EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), YAML::BadConversion); + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); animation_description["repeat"] = "1.1"; - EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), YAML::BadConversion); + EXPECT_THROW(animation_->Initialize(animation_description, 10, 10.0), std::runtime_error); // exceeded anim display duration animation_description["repeat"] = "6"; diff --git a/panther_lights/test/test_led_segment.cpp b/panther_lights/test/test_led_segment.cpp index 561dfb012..196359347 100644 --- a/panther_lights/test/test_led_segment.cpp +++ b/panther_lights/test/test_led_segment.cpp @@ -54,25 +54,25 @@ TEST(TestLEDSegmentInitialization, DescriptionMissingRequiredKey) auto segment_desc = YAML::Load(""); EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, - "Missing 'led_range' in segment description")); + "Missing 'channel' in description")); - segment_desc = YAML::Load("led_range: 0-10"); + segment_desc = YAML::Load("channel: 0"); EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, - "Missing 'channel' in segment description")); + "Missing 'led_range' in description")); } TEST(TestLEDSegmentInitialization, InvalidChannelExpression) { auto segment_desc = CreateSegmentDescription("0-10", "s1"); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, - "Invalid channel expression: ")); + "Failed to convert 'channel' key")); segment_desc["channel"] = "-1"; - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [segment_desc]() { panther_lights::LEDSegment(segment_desc, 10.0); }, - "Invalid channel expression: ")); + "Failed to convert 'channel' key")); } TEST(TestLEDSegmentInitialization, InvalidLedRangeExpression) @@ -128,41 +128,36 @@ TEST(TestLEDSegmentInitialization, FirstLedPosition) EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); } -TEST_F(TestLEDSegment, SetAnimationMissingTypeKey) -{ - const auto animation_desc = YAML::Load(""); - EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { led_segment_->SetAnimation(animation_desc); }, - "Missing 'type' in animation description")); -} - TEST_F(TestLEDSegment, SetAnimationInvalidType) { - const auto animation_desc = YAML::Load("{type: panther_lights::WrongAnimationType}"); + const YAML::Node animation_desc; EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { led_segment_->SetAnimation(animation_desc); }, "The plugin failed to load. Error: ")); + [&]() { + led_segment_->SetAnimation("panther_lights::WrongAnimationType}", animation_desc, false); + }, + "The plugin failed to load. Error: ")); } TEST_F(TestLEDSegment, SetAnimationFailAnimationInitialization) { const auto animation_desc = YAML::Load("{type: panther_lights::ImageAnimation}"); EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( - [&]() { led_segment_->SetAnimation(animation_desc); }, "Failed to initialize animation: ")); + [&]() { led_segment_->SetAnimation("panther_lights::ImageAnimation", animation_desc, false); }, + "Failed to initialize animation: ")); } TEST_F(TestLEDSegment, SetAnimation) { // test each known animtion type const auto image_anim_desc = YAML::Load( - "{type: panther_lights::ImageAnimation, " - "image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find panther_lights)/animations/triangle01_red.png, " "duration: 2}"); - EXPECT_NO_THROW(led_segment_->SetAnimation(image_anim_desc)); + EXPECT_NO_THROW( + led_segment_->SetAnimation("panther_lights::ImageAnimation", image_anim_desc, false)); - const auto charging_anim_desc = YAML::Load( - "{type: panther_lights::ChargingAnimation, " - "duration: 2}"); - EXPECT_NO_THROW(led_segment_->SetAnimation(charging_anim_desc)); + const auto charging_anim_desc = YAML::Load("{duration: 2}"); + EXPECT_NO_THROW(led_segment_->SetAnimation( + "panther_lights::ChargingAnimation", charging_anim_desc, false, "0.5")); } TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet) @@ -174,10 +169,9 @@ TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet) TEST_F(TestLEDSegment, UpdateAnimation) { const auto anim_desc = YAML::Load( - "{type: panther_lights::ImageAnimation, " - "image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find panther_lights)/animations/triangle01_red.png, " "duration: 2}"); - ASSERT_NO_THROW(led_segment_->SetAnimation(anim_desc)); + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); EXPECT_NO_THROW(led_segment_->UpdateAnimation()); EXPECT_EQ(segment_led_num_ * 4, led_segment_->GetAnimationFrame().size()); } diff --git a/panther_lights/test/test_segment_converter.cpp b/panther_lights/test/test_segment_converter.cpp index 8636f467b..407356106 100644 --- a/panther_lights/test/test_segment_converter.cpp +++ b/panther_lights/test/test_segment_converter.cpp @@ -48,7 +48,7 @@ class TestSegmentConverter : public testing::Test std::size_t panel_2_num_led_ = 30; std::unique_ptr segment_converter_; - std::vector> segments_; + std::unordered_map> segments_; std::unordered_map> led_panels_; }; @@ -64,43 +64,49 @@ YAML::Node TestSegmentConverter::CreateSegmentDescription( YAML::Node TestSegmentConverter::CreateImageAnimationDescription() { return YAML::Load( - "{type: panther_lights::ImageAnimation, " - "image: $(find panther_lights)/animations/triangle01_red.png, " + "{image: $(find panther_lights)/animations/triangle01_red.png, " "duration: 2}"); } TEST_F(TestSegmentConverter, ConvertInvalidChannel) { - segments_.push_back( + segments_.emplace( + "name", std::make_shared(CreateSegmentDescription(0, 10, 123), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); - ASSERT_NO_THROW(segments_.at(0)->SetAnimation(anim_desc)); + ASSERT_NO_THROW( + segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::out_of_range); } TEST_F(TestSegmentConverter, ConvertInvalidLedRange) { - segments_.push_back(std::make_shared( - CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 10, 1), 50.0)); + segments_.emplace( + "name", std::make_shared( + CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 10, 1), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); - ASSERT_NO_THROW(segments_.at(0)->SetAnimation(anim_desc)); + ASSERT_NO_THROW( + segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); EXPECT_THROW(segment_converter_->Convert(segments_, led_panels_), std::runtime_error); } TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel) { - segments_.push_back(std::make_shared( - CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); - segments_.push_back(std::make_shared( - CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); + segments_.emplace( + "name_1", std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); + segments_.emplace( + "name_2", std::make_shared( + CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); for (auto & segment : segments_) { - ASSERT_NO_THROW(segment->SetAnimation(anim_desc)); - ASSERT_NO_THROW(segment->UpdateAnimation()); + ASSERT_NO_THROW( + segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW(segment.second->UpdateAnimation()); } EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); @@ -108,22 +114,30 @@ TEST_F(TestSegmentConverter, ConvertSingleSegmentForEachPanel) TEST_F(TestSegmentConverter, ConvertMultipleSegments) { - segments_.push_back(std::make_shared( - CreateSegmentDescription(0, std::size_t(panel_1_num_led_ / 2) - 1, 1), 50.0)); - segments_.push_back(std::make_shared( - CreateSegmentDescription(std::size_t(panel_1_num_led_ / 2), panel_1_num_led_ - 1, 1), 50.0)); - - segments_.push_back(std::make_shared( - CreateSegmentDescription(0, (panel_2_num_led_ / 4) - 1, 2), 50.0)); - segments_.push_back(std::make_shared( - CreateSegmentDescription((panel_2_num_led_ / 4), (panel_2_num_led_ / 2) - 1, 2), 50.0)); - segments_.push_back(std::make_shared( - CreateSegmentDescription((panel_2_num_led_ / 2), panel_2_num_led_ - 1, 2), 50.0)); + segments_.emplace( + "name_1", std::make_shared( + CreateSegmentDescription(0, std::size_t(panel_1_num_led_ / 2) - 1, 1), 50.0)); + segments_.emplace( + "name_2", + std::make_shared( + CreateSegmentDescription(std::size_t(panel_1_num_led_ / 2), panel_1_num_led_ - 1, 1), 50.0)); + + segments_.emplace( + "name_3", std::make_shared( + CreateSegmentDescription(0, (panel_2_num_led_ / 4) - 1, 2), 50.0)); + segments_.emplace( + "name_4", + std::make_shared( + CreateSegmentDescription((panel_2_num_led_ / 4), (panel_2_num_led_ / 2) - 1, 2), 50.0)); + segments_.emplace( + "name_5", std::make_shared( + CreateSegmentDescription((panel_2_num_led_ / 2), panel_2_num_led_ - 1, 2), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); for (auto & segment : segments_) { - ASSERT_NO_THROW(segment->SetAnimation(anim_desc)); - ASSERT_NO_THROW(segment->UpdateAnimation()); + ASSERT_NO_THROW( + segment.second->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + ASSERT_NO_THROW(segment.second->UpdateAnimation()); } EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); From 7afb6881d985be9981b94e91b8ac17882b18539a Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 27 Feb 2024 11:07:44 +0000 Subject: [PATCH 16/32] update tests --- .../include/panther_lights/led_segment.hpp | 6 +- panther_lights/src/segment_converter.cpp | 1 - panther_lights/test/test_led_segment.cpp | 76 ++++++++++++++++++- .../test/test_segment_converter.cpp | 37 +++++++++ 4 files changed, 115 insertions(+), 5 deletions(-) diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 3019149ea..277eedfe6 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -116,6 +116,10 @@ class LEDSegment bool HasAnimation() const { return animation_ || default_animation_; } +protected: + std::shared_ptr animation_; + std::shared_ptr default_animation_; + private: const float controller_frequency_; bool invert_led_order_ = false; @@ -125,8 +129,6 @@ class LEDSegment std::size_t last_led_iterator_; std::size_t num_led_; - std::shared_ptr animation_; - std::shared_ptr default_animation_; std::shared_ptr> animation_loader_; }; diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp index 22a600329..d8ace49e7 100644 --- a/panther_lights/src/segment_converter.cpp +++ b/panther_lights/src/segment_converter.cpp @@ -33,7 +33,6 @@ void SegmentConverter::Convert( for (auto & [segment_name, segment] : segments) { if (!segment->HasAnimation()) { continue; - ; } try { diff --git a/panther_lights/test/test_led_segment.cpp b/panther_lights/test/test_led_segment.cpp index 196359347..dec99a6f2 100644 --- a/panther_lights/test/test_led_segment.cpp +++ b/panther_lights/test/test_led_segment.cpp @@ -24,6 +24,21 @@ #include #include +class LEDSegmentWrapper : public panther_lights::LEDSegment +{ +public: + LEDSegmentWrapper(const YAML::Node & segment_description, const float controller_frequency) + : LEDSegment(segment_description, controller_frequency) + { + } + + std::shared_ptr GetAnimation() const { return animation_; } + std::shared_ptr GetDefaultAnimation() const + { + return default_animation_; + } +}; + class TestLEDSegment : public testing::Test { public: @@ -31,7 +46,7 @@ class TestLEDSegment : public testing::Test ~TestLEDSegment() {} protected: - std::shared_ptr led_segment_; + std::shared_ptr led_segment_; const float controller_freq = 50.0; const std::size_t segment_led_num_ = 10; @@ -41,7 +56,7 @@ TestLEDSegment::TestLEDSegment() { const auto segment_desc = YAML::Load("{led_range: 0-" + std::to_string(segment_led_num_ - 1) + ", channel: 1}"); - led_segment_ = std::make_shared(segment_desc, controller_freq); + led_segment_ = std::make_shared(segment_desc, controller_freq); } YAML::Node CreateSegmentDescription(const std::string & led_range, const std::string & channel) @@ -128,6 +143,30 @@ TEST(TestLEDSegmentInitialization, FirstLedPosition) EXPECT_EQ(std::size_t(0), led_segment->GetFirstLEDPosition()); } +TEST_F(TestLEDSegment, GetAnimationFrameNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->GetAnimationFrame(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, GetAnimationProgressNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->GetAnimationProgress(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, ResetAnimationNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->ResetAnimation(); }, "Segment animation not defined")); +} + +TEST_F(TestLEDSegment, GetAnimationBrightnessNoAnimation) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { led_segment_->GetAnimationBrightness(); }, "Segment animation not defined")); +} + TEST_F(TestLEDSegment, SetAnimationInvalidType) { const YAML::Node animation_desc; @@ -160,6 +199,21 @@ TEST_F(TestLEDSegment, SetAnimation) "panther_lights::ChargingAnimation", charging_anim_desc, false, "0.5")); } +TEST_F(TestLEDSegment, SetAnimationRepeating) +{ + const auto anim_desc = YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() == nullptr); + + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true)); + + EXPECT_TRUE(led_segment_->GetDefaultAnimation().get() != nullptr); + EXPECT_TRUE(led_segment_->IsAnimationFinished()); +} + TEST_F(TestLEDSegment, UpdateAnimationAnimationNotSet) { EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( @@ -181,3 +235,21 @@ int main(int argc, char ** argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } + +TEST_F(TestLEDSegment, ResetDefaultAnimationWhenNewArrive) +{ + const auto anim_desc = YAML::Load( + "{image: $(find panther_lights)/animations/triangle01_red.png, " + "duration: 2}"); + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, true)); + + auto default_anim = led_segment_->GetDefaultAnimation(); + while (!default_anim->IsFinished()) { + ASSERT_NO_THROW(led_segment_->UpdateAnimation()); + } + + // add new animation, and check if default animation was reset + ASSERT_NO_THROW(led_segment_->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + EXPECT_FALSE(default_anim->IsFinished()); +} diff --git a/panther_lights/test/test_segment_converter.cpp b/panther_lights/test/test_segment_converter.cpp index 407356106..9540a7635 100644 --- a/panther_lights/test/test_segment_converter.cpp +++ b/panther_lights/test/test_segment_converter.cpp @@ -143,6 +143,43 @@ TEST_F(TestSegmentConverter, ConvertMultipleSegments) EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); } +TEST_F(TestSegmentConverter, ConvertBrightnessOverride) +{ + const std::size_t channel = 1; + const float float_brightness = 0.2f; + const std::uint8_t expected_brightness = static_cast(round(float_brightness * 255)); + auto anim_desc = CreateImageAnimationDescription(); + anim_desc["brightness"] = float_brightness; + + segments_.emplace( + "name", std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, channel), 50.0)); + + ASSERT_NO_THROW( + segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); + + segment_converter_->Convert(segments_, led_panels_); + ASSERT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); + + const auto frame = led_panels_.at(channel)->GetFrame(); + + for (std::size_t i = 3; i < frame.size(); i += 4) { + EXPECT_EQ(expected_brightness, frame[i]); + } +} + +TEST_F(TestSegmentConverter, ConvertNoThrowIfAnimationNotSet) +{ + segments_.emplace( + "name_1", std::make_shared( + CreateSegmentDescription(0, panel_1_num_led_ - 1, 1), 50.0)); + segments_.emplace( + "name_2", std::make_shared( + CreateSegmentDescription(0, panel_2_num_led_ - 1, 2), 50.0)); + + EXPECT_NO_THROW(segment_converter_->Convert(segments_, led_panels_)); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From fa78597becf1729d040a31cd2079d75ec2016650 Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 27 Feb 2024 14:29:16 +0000 Subject: [PATCH 17/32] add led_animation test --- panther_lights/CMakeLists.txt | 11 ++ .../include/panther_lights/led_segment.hpp | 2 +- panther_lights/src/led_segment.cpp | 3 +- panther_lights/test/test_led_animation.cpp | 152 ++++++++++++++++++ 4 files changed, 166 insertions(+), 2 deletions(-) create mode 100644 panther_lights/test/test_led_animation.cpp diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 4fc2652e5..465053f95 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -125,6 +125,17 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_segment_converter pluginlib panther_utils) target_link_libraries(${PROJECT_NAME}_test_segment_converter yaml-cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_led_animation test/test_led_animation.cpp + src/led_panel.cpp src/led_segment.cpp src/led_animations_queue.cpp) + target_include_directories( + ${PROJECT_NAME}_test_led_animation + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_led_animation pluginlib + panther_utils rclcpp) + target_link_libraries(${PROJECT_NAME}_test_led_animation yaml-cpp) endif() ament_package() diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 277eedfe6..05e9bdb7d 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -101,7 +101,7 @@ class LEDSegment * * @exception std::runtime_error if segment animation is not defined */ - void ResetAnimation() const; + void ResetAnimation(); /** * @brief Get current animation brightness diff --git a/panther_lights/src/led_segment.cpp b/panther_lights/src/led_segment.cpp index 7741f837e..6827afd38 100644 --- a/panther_lights/src/led_segment.cpp +++ b/panther_lights/src/led_segment.cpp @@ -143,13 +143,14 @@ float LEDSegment::GetAnimationProgress() const return animation_->GetProgress(); } -void LEDSegment::ResetAnimation() const +void LEDSegment::ResetAnimation() { if (!animation_) { throw std::runtime_error("Segment animation not defined"); } animation_->Reset(); + animation_finished_ = false; } std::uint8_t LEDSegment::GetAnimationBrightness() const diff --git a/panther_lights/test/test_led_animation.cpp b/panther_lights/test/test_led_animation.cpp new file mode 100644 index 000000000..c4fe60771 --- /dev/null +++ b/panther_lights/test/test_led_animation.cpp @@ -0,0 +1,152 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +class TestLEDAnimation : public testing::Test +{ +public: + TestLEDAnimation(); + ~TestLEDAnimation() {} + + void SetSegmentAnimations(); + +protected: + std::shared_ptr led_anim_; + std::unordered_map> segments_; +}; + +TestLEDAnimation::TestLEDAnimation() +{ + auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); + auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); + segments_.emplace( + "segment_1", std::make_shared(segment_1_desc, 50.0)); + segments_.emplace( + "segment_2", std::make_shared(segment_2_desc, 50.0)); + + panther_lights::AnimationDescription anim_desc; + anim_desc.segments = {"segment_1", "segment_2"}; + anim_desc.type = "panther_lights::ImageAnimation"; + anim_desc.animation = + YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); + + panther_lights::LEDAnimationDescription led_anim_desc; + led_anim_desc.id = 0; + led_anim_desc.name = "ulala"; + led_anim_desc.priority = 1; + led_anim_desc.timeout = 10.0; + led_anim_desc.animations = {anim_desc}; + + led_anim_ = std::make_shared( + led_anim_desc, segments_, rclcpp::Time(0)); +} + +void TestLEDAnimation::SetSegmentAnimations() +{ + const auto animations = led_anim_->GetAnimations(); + for (auto & animation : animations) { + for (auto & segment : animation.segments) { + segments_.at(segment)->SetAnimation( + animation.type, animation.animation, led_anim_->IsRepeating(), led_anim_->GetParam()); + } + } +} + +TEST_F(TestLEDAnimation, IsFinished) +{ + SetSegmentAnimations(); + + EXPECT_FALSE(led_anim_->IsFinished()); + + while (!segments_.at("segment_1")->IsAnimationFinished()) { + segments_.at("segment_1")->UpdateAnimation(); + } + + EXPECT_FALSE(led_anim_->IsFinished()); + + while (!segments_.at("segment_2")->IsAnimationFinished()) { + segments_.at("segment_2")->UpdateAnimation(); + } + + EXPECT_TRUE(led_anim_->IsFinished()); +} + +TEST_F(TestLEDAnimation, GetProgress) +{ + SetSegmentAnimations(); + + EXPECT_FLOAT_EQ(0.0, led_anim_->GetProgress()); + + while (!segments_.at("segment_1")->IsAnimationFinished()) { + segments_.at("segment_1")->UpdateAnimation(); + } + + EXPECT_FLOAT_EQ(0.0, led_anim_->GetProgress()); + EXPECT_FALSE(led_anim_->IsFinished()); + + while (!segments_.at("segment_2")->IsAnimationFinished()) { + segments_.at("segment_2")->UpdateAnimation(); + } + + EXPECT_FLOAT_EQ(1.0, led_anim_->GetProgress()); +} + +TEST_F(TestLEDAnimation, Reset) +{ + SetSegmentAnimations(); + + while (segments_.at("segment_1")->IsAnimationFinished()) { + segments_.at("segment_1")->UpdateAnimation(); + } + + while (segments_.at("segment_2")->IsAnimationFinished()) { + segments_.at("segment_2")->UpdateAnimation(); + } + + EXPECT_TRUE(led_anim_->GetInitTime() == rclcpp::Time(0)); + EXPECT_FALSE(led_anim_->IsFinished()); + EXPECT_FALSE(segments_.at("segment_1")->IsAnimationFinished()); + EXPECT_FALSE(segments_.at("segment_2")->IsAnimationFinished()); + EXPECT_NE(1.0, segments_.at("segment_1")->GetAnimationProgress()); + EXPECT_NE(1.0, segments_.at("segment_2")->GetAnimationProgress()); + + auto reset_time = rclcpp::Time(1); + led_anim_->Reset(reset_time); + + EXPECT_TRUE(led_anim_->GetInitTime() == reset_time); + EXPECT_FALSE(led_anim_->IsFinished()); + EXPECT_FALSE(segments_.at("segment_1")->IsAnimationFinished()); + EXPECT_FALSE(segments_.at("segment_2")->IsAnimationFinished()); + EXPECT_FLOAT_EQ(0.0, segments_.at("segment_1")->GetAnimationProgress()); + EXPECT_FLOAT_EQ(0.0, segments_.at("segment_2")->GetAnimationProgress()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From cc83afaa9b471b68f572878775517372bfe205ab Mon Sep 17 00:00:00 2001 From: Dawid Date: Thu, 29 Feb 2024 07:08:22 +0000 Subject: [PATCH 18/32] test fixxes --- .../src/animation/charging_animation.cpp | 2 +- panther_lights/test/test_charging_animation.cpp | 2 +- panther_lights/test/test_led_animation.cpp | 14 +++++++------- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/panther_lights/src/animation/charging_animation.cpp b/panther_lights/src/animation/charging_animation.cpp index 1e1cefc3e..356836f46 100644 --- a/panther_lights/src/animation/charging_animation.cpp +++ b/panther_lights/src/animation/charging_animation.cpp @@ -43,7 +43,7 @@ void ChargingAnimation::SetParam(const std::string & param) try { battery_percent = std::clamp(std::stof(param), 0.0f, 1.0f); } catch (const std::invalid_argument & /*e*/) { - throw std::invalid_argument("Can not cast param to float!"); + throw std::runtime_error("Can not cast param to float!"); } const auto anim_len = this->GetAnimationLength(); diff --git a/panther_lights/test/test_charging_animation.cpp b/panther_lights/test/test_charging_animation.cpp index 093dfa431..b25123d99 100644 --- a/panther_lights/test/test_charging_animation.cpp +++ b/panther_lights/test/test_charging_animation.cpp @@ -147,7 +147,7 @@ TEST_F(TestChargingAnimation, CreateRGBAFrame) TEST_F(TestChargingAnimation, SetParam) { - EXPECT_THROW(animation_->SetParam("not_a_number"), std::invalid_argument); + EXPECT_THROW(animation_->SetParam("not_a_number"), std::runtime_error); EXPECT_NO_THROW(animation_->SetParam("0.7")); } diff --git a/panther_lights/test/test_led_animation.cpp b/panther_lights/test/test_led_animation.cpp index c4fe60771..574d2c799 100644 --- a/panther_lights/test/test_led_animation.cpp +++ b/panther_lights/test/test_led_animation.cpp @@ -119,20 +119,20 @@ TEST_F(TestLEDAnimation, Reset) { SetSegmentAnimations(); - while (segments_.at("segment_1")->IsAnimationFinished()) { + while (!segments_.at("segment_1")->IsAnimationFinished()) { segments_.at("segment_1")->UpdateAnimation(); } - while (segments_.at("segment_2")->IsAnimationFinished()) { + while (!segments_.at("segment_2")->IsAnimationFinished()) { segments_.at("segment_2")->UpdateAnimation(); } EXPECT_TRUE(led_anim_->GetInitTime() == rclcpp::Time(0)); - EXPECT_FALSE(led_anim_->IsFinished()); - EXPECT_FALSE(segments_.at("segment_1")->IsAnimationFinished()); - EXPECT_FALSE(segments_.at("segment_2")->IsAnimationFinished()); - EXPECT_NE(1.0, segments_.at("segment_1")->GetAnimationProgress()); - EXPECT_NE(1.0, segments_.at("segment_2")->GetAnimationProgress()); + EXPECT_TRUE(led_anim_->IsFinished()); + EXPECT_TRUE(segments_.at("segment_1")->IsAnimationFinished()); + EXPECT_TRUE(segments_.at("segment_2")->IsAnimationFinished()); + EXPECT_FLOAT_EQ(1.0, segments_.at("segment_1")->GetAnimationProgress()); + EXPECT_FLOAT_EQ(1.0, segments_.at("segment_2")->GetAnimationProgress()); auto reset_time = rclcpp::Time(1); led_anim_->Reset(reset_time); From 6a074a95c20ba08c6855ef8f56778935fe30b5bc Mon Sep 17 00:00:00 2001 From: Dawid Date: Thu, 29 Feb 2024 09:16:00 +0000 Subject: [PATCH 19/32] add led animations queue tests --- panther_lights/CMakeLists.txt | 12 + panther_lights/src/led_animations_queue.cpp | 2 +- panther_lights/test/test_led_animation.cpp | 2 +- .../test/test_led_animations_queue.cpp | 268 ++++++++++++++++++ 4 files changed, 282 insertions(+), 2 deletions(-) create mode 100644 panther_lights/test/test_led_animations_queue.cpp diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 465053f95..2ef7d7a47 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -136,6 +136,18 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_led_animation pluginlib panther_utils rclcpp) target_link_libraries(${PROJECT_NAME}_test_led_animation yaml-cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_led_animations_queue + test/test_led_animations_queue.cpp src/led_panel.cpp src/led_segment.cpp + src/led_animations_queue.cpp) + target_include_directories( + ${PROJECT_NAME}_test_led_animations_queue + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_led_animations_queue pluginlib + panther_utils rclcpp) + target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp) endif() ament_package() diff --git a/panther_lights/src/led_animations_queue.cpp b/panther_lights/src/led_animations_queue.cpp index 305386785..197cf45ae 100644 --- a/panther_lights/src/led_animations_queue.cpp +++ b/panther_lights/src/led_animations_queue.cpp @@ -103,7 +103,7 @@ std::shared_ptr LEDAnimationsQueue::Get() throw std::runtime_error("Queue empty"); } -void LEDAnimationsQueue::Clear(std::size_t priority) +void LEDAnimationsQueue::Clear(const std::size_t priority) { const auto new_end = std::remove_if( queue_.begin(), queue_.end(), [priority](const std::shared_ptr & animation) { diff --git a/panther_lights/test/test_led_animation.cpp b/panther_lights/test/test_led_animation.cpp index 574d2c799..094b98242 100644 --- a/panther_lights/test/test_led_animation.cpp +++ b/panther_lights/test/test_led_animation.cpp @@ -56,7 +56,7 @@ TestLEDAnimation::TestLEDAnimation() panther_lights::LEDAnimationDescription led_anim_desc; led_anim_desc.id = 0; - led_anim_desc.name = "ulala"; + led_anim_desc.name = "TEST"; led_anim_desc.priority = 1; led_anim_desc.timeout = 10.0; led_anim_desc.animations = {anim_desc}; diff --git a/panther_lights/test/test_led_animations_queue.cpp b/panther_lights/test/test_led_animations_queue.cpp new file mode 100644 index 000000000..8ce89ce40 --- /dev/null +++ b/panther_lights/test/test_led_animations_queue.cpp @@ -0,0 +1,268 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +class TestLEDAnimationsQueue : public testing::Test +{ +public: + TestLEDAnimationsQueue(); + ~TestLEDAnimationsQueue() {} + + panther_lights::LEDAnimation CreateLEDAnimation( + const std::string & name, const std::uint8_t priority, + const rclcpp::Time & init_time = rclcpp::Time(0)); + +protected: + std::shared_ptr led_anim_queue_; + std::unordered_map> segments_; + + const std::size_t max_queue_size_ = 5; +}; + +TestLEDAnimationsQueue::TestLEDAnimationsQueue() +{ + auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); + auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); + segments_.emplace( + "segment_1", std::make_shared(segment_1_desc, 50.0)); + segments_.emplace( + "segment_2", std::make_shared(segment_2_desc, 50.0)); + + led_anim_queue_ = std::make_shared(5); +} + +panther_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation( + const std::string & name, const std::uint8_t priority, const rclcpp::Time & init_time) +{ + panther_lights::AnimationDescription anim_desc; + anim_desc.segments = {"segment_1", "segment_2"}; + anim_desc.type = "panther_lights::ImageAnimation"; + anim_desc.animation = + YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); + + panther_lights::LEDAnimationDescription led_anim_desc; + led_anim_desc.id = 0; + led_anim_desc.name = name; + led_anim_desc.priority = priority; + led_anim_desc.timeout = 10.0; + led_anim_desc.animations = {anim_desc}; + + return panther_lights::LEDAnimation(led_anim_desc, segments_, init_time); +} + +TEST_F(TestLEDAnimationsQueue, Put) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); + led_anim_queue_->Put(led_anim, rclcpp::Time(0)); + + EXPECT_FALSE(led_anim_queue_->Empty()); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim)); +} + +TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); + for (std::size_t i = 0; i < max_queue_size_; i++) { + led_anim_queue_->Put(led_anim, rclcpp::Time(0)); + } + + EXPECT_THROW(led_anim_queue_->Put(led_anim, rclcpp::Time(0)), std::runtime_error); +} + +TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST", 3)); + + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_1); + // animation with priority 1 should remove animations with lower priorities + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +TEST_F(TestLEDAnimationsQueue, PutSortByPriority) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST", 3)); + + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_2); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_2); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_3); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_pr_3); +} + +TEST_F(TestLEDAnimationsQueue, PutSortByTime) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim_t0 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); + auto led_anim_t1 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(1))); + auto led_anim_t2 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(2))); + auto led_anim_t3 = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(3))); + + led_anim_queue_->Put(led_anim_t3, rclcpp::Time(4)); + led_anim_queue_->Put(led_anim_t1, rclcpp::Time(4)); + led_anim_queue_->Put(led_anim_t2, rclcpp::Time(4)); + led_anim_queue_->Put(led_anim_t0, rclcpp::Time(4)); + + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t0); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t1); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t2); + EXPECT_TRUE(led_anim_queue_->Get() == led_anim_t3); +} + +TEST_F(TestLEDAnimationsQueue, GetQueueEmpty) +{ + EXPECT_THROW(led_anim_queue_->Get(), std::runtime_error); +} + +TEST_F(TestLEDAnimationsQueue, Clear) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST1", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST2", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST3", 3)); + + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + + led_anim_queue_->Clear(3); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim_pr_2)); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_3)); + + led_anim_queue_->Clear(); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_2)); + + led_anim_queue_->Clear(1); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim = + std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); + + led_anim_queue_->Put(led_anim, rclcpp::Time(0)); + led_anim_queue_->Validate(rclcpp::Time(0)); + EXPECT_TRUE(led_anim_queue_->HasAnimation(led_anim)); + + // exceed timeout + led_anim_queue_->Validate(rclcpp::Time(12, 0)); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim)); + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST1", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST2", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST3", 3)); + + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 1); + led_anim_queue_->Get(); + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 2); + led_anim_queue_->Get(); + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 3); + led_anim_queue_->Get(); + EXPECT_TRUE(led_anim_queue_->Empty()); + EXPECT_TRUE(led_anim_queue_->GetFirstAnimationPriority() == 3); +} + +TEST_F(TestLEDAnimationsQueue, Remove) +{ + EXPECT_TRUE(led_anim_queue_->Empty()); + + auto led_anim_pr_1 = + std::make_shared(CreateLEDAnimation("TEST1", 1)); + auto led_anim_pr_2 = + std::make_shared(CreateLEDAnimation("TEST2", 2)); + auto led_anim_pr_3 = + std::make_shared(CreateLEDAnimation("TEST3", 3)); + + led_anim_queue_->Put(led_anim_pr_1, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_2, rclcpp::Time(0)); + led_anim_queue_->Put(led_anim_pr_3, rclcpp::Time(0)); + + led_anim_queue_->Remove(led_anim_pr_2); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_2)); + led_anim_queue_->Remove(led_anim_pr_3); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_3)); + led_anim_queue_->Remove(led_anim_pr_1); + EXPECT_FALSE(led_anim_queue_->HasAnimation(led_anim_pr_1)); + EXPECT_TRUE(led_anim_queue_->Empty()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From dcaef20ee3ccd94803d56da81186bec3fec0cf8e Mon Sep 17 00:00:00 2001 From: Dawid Date: Fri, 1 Mar 2024 14:49:33 +0000 Subject: [PATCH 20/32] clean up code | clean up code --- panther_lights/CMakeLists.txt | 16 + .../panther_lights/controller_node.hpp | 12 +- .../panther_lights/led_animations_queue.hpp | 13 +- panther_lights/src/controller_node.cpp | 27 +- panther_lights/src/led_animations_queue.cpp | 11 +- panther_lights/test/test_controller_node.cpp | 325 ++++++++++++++++++ panther_lights/test/test_led_animation.cpp | 15 + 7 files changed, 379 insertions(+), 40 deletions(-) create mode 100644 panther_lights/test/test_controller_node.cpp diff --git a/panther_lights/CMakeLists.txt b/panther_lights/CMakeLists.txt index 2ef7d7a47..2e1e9a475 100644 --- a/panther_lights/CMakeLists.txt +++ b/panther_lights/CMakeLists.txt @@ -148,6 +148,22 @@ if(BUILD_TESTING) ament_target_dependencies(${PROJECT_NAME}_test_led_animations_queue pluginlib panther_utils rclcpp) target_link_libraries(${PROJECT_NAME}_test_led_animations_queue yaml-cpp) + + ament_add_gtest( + ${PROJECT_NAME}_test_controller_node + test/test_controller_node.cpp + src/controller_node.cpp + src/led_segment.cpp + src/led_panel.cpp + src/segment_converter.cpp + src/led_animations_queue.cpp) + target_include_directories( + ${PROJECT_NAME}_test_controller_node + PUBLIC $ + $) + ament_target_dependencies(${PROJECT_NAME}_test_controller_node pluginlib + panther_msgs panther_utils rclcpp sensor_msgs) + target_link_libraries(${PROJECT_NAME}_test_controller_node yaml-cpp) endif() ament_package() diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index c5ec3fa35..629360d9d 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -45,10 +45,7 @@ class ControllerNode : public rclcpp::Node const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); ~ControllerNode() {} -private: - void DeclareParameters(); - void LoadParameters(); - +protected: /** * @brief Initializes LED panel based on YAML description. This LED panel is a representation of * the real panel of the robot @@ -144,6 +141,10 @@ class ControllerNode : public rclcpp::Node */ void SetLEDAnimation(const std::shared_ptr & led_animation); + std::shared_ptr current_animation_; + std::shared_ptr animations_queue_; + +private: void PublishPanelFrame(const std::size_t channel); void SetLEDAnimationCB( const SetLEDAnimationSrv::Request::SharedPtr & request, @@ -161,9 +162,6 @@ class ControllerNode : public rclcpp::Node rclcpp::Service::SharedPtr set_led_animation_server_; rclcpp::TimerBase::SharedPtr controller_timer_; - std::shared_ptr current_animation_; - std::unique_ptr animations_queue_; - std::mutex queue_mtx_; bool animation_finished_ = true; diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp index 08f9f6fbf..2ff0a440a 100644 --- a/panther_lights/include/panther_lights/led_animations_queue.hpp +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -25,7 +25,7 @@ #include -#include +#include #include @@ -104,7 +104,6 @@ class LEDAnimation void SetRepeating(const bool value) { repeating_ = value; } void SetParam(const std::string & param) { param_ = param; } - static constexpr char kDefaultName[] = "UNDEFINED"; static constexpr std::uint8_t kDefaultPriority = 3; static constexpr float kDefaultTimeout = 120.0f; static constexpr std::array kValidPriorities = {1, 2, 3}; @@ -156,7 +155,7 @@ class LEDAnimationsQueue * * @param priority Animation with this priority or lower will be removed from the queue */ - void Clear(std::size_t priority = 2); + void Clear(const std::size_t priority = 2); /** * @brief Removes animations that has reached theirs timeout from the queue @@ -177,14 +176,6 @@ class LEDAnimationsQueue bool Empty() const { return queue_.empty(); } - void Print() - { - std::cout << "--------" << std::endl; - for (auto & anim : queue_) { - std::cout << anim->GetName() << std::endl; - } - } - private: std::deque> queue_; const std::size_t max_queue_size_; diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index fedb5ffdb..7812c83fc 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -46,6 +46,7 @@ using std::placeholders::_2; ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { + // TODO, move default led_config_file to launch this->declare_parameter( "led_config_file", "/home/husarion/ros2_ws/src/panther_ros/panther_lights/config/led_config.yaml"); @@ -61,7 +62,7 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node InitializeLEDPanels(led_config_desc["panels"]); InitializeLEDSegments(led_config_desc["segments"], controller_freq); InitializeLEDSegmentsMap(led_config_desc["segments_map"]); - LoadDefaultAnimations(led_config_desc["animations"]); + LoadDefaultAnimations(led_config_desc["led_animations"]); if (user_led_animations_file != "") { LoadUserAnimations(user_led_animations_file); @@ -69,7 +70,7 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node segment_converter_ = std::make_shared(); - animations_queue_ = std::make_unique(10); + animations_queue_ = std::make_shared(10); set_led_animation_server_ = this->create_service( "lights/controller/set/animation", std::bind(&ControllerNode::SetLEDAnimationCB, this, _1, _2)); @@ -79,19 +80,6 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node std::bind(&ControllerNode::ControllerTimerCB, this)); RCLCPP_INFO(this->get_logger(), "Node started"); - - // animations are initialized when they are set. No way do check if they are correctly defined - // before? -} - -void ControllerNode::DeclareParameters() -{ - // this->declare_parameter("led_config_file"); -} - -void ControllerNode::LoadParameters() -{ - // led_config_file_ = this->get_parameter("led_config_file").as_string(); } void ControllerNode::InitializeLEDPanels(const YAML::Node & panels_description) @@ -206,10 +194,10 @@ void ControllerNode::LoadAnimation(const YAML::Node & animation_description) LEDAnimationDescription led_animation_desc; try { - led_animation_desc.name = panther_utils::GetYAMLKeyValue( - animation_description, "name", LEDAnimation::kDefaultName); led_animation_desc.id = panther_utils::GetYAMLKeyValue( animation_description, "id"); + led_animation_desc.name = panther_utils::GetYAMLKeyValue( + animation_description, "name", "ANIMATION_" + std::to_string(led_animation_desc.id)); led_animation_desc.priority = panther_utils::GetYAMLKeyValue( animation_description, "priority", LEDAnimation::kDefaultPriority); led_animation_desc.timeout = panther_utils::GetYAMLKeyValue( @@ -219,7 +207,7 @@ void ControllerNode::LoadAnimation(const YAML::Node & animation_description) std::find( LEDAnimation::kValidPriorities.begin(), LEDAnimation::kValidPriorities.end(), led_animation_desc.priority) == LEDAnimation::kValidPriorities.end()) { - throw std::runtime_error("Invalid LED animation ID"); + throw std::runtime_error("Invalid LED animation priority"); } auto animations = panther_utils::GetYAMLKeyValue>( @@ -237,7 +225,7 @@ void ControllerNode::LoadAnimation(const YAML::Node & animation_description) const auto result = animations_descriptions_.emplace(led_animation_desc.id, led_animation_desc); if (!result.second) { - throw std::runtime_error("Animation with given ID already exists."); + throw std::runtime_error("Animation with given ID already exists"); } } catch (const std::runtime_error & e) { @@ -352,7 +340,6 @@ void ControllerNode::AddAnimationToQueue( animation->SetRepeating(repeating); animation->SetParam(param); animations_queue_->Put(animation, this->get_clock()->now()); - animations_queue_->Print(); } void ControllerNode::SetLEDAnimation(const std::shared_ptr & led_animation) diff --git a/panther_lights/src/led_animations_queue.cpp b/panther_lights/src/led_animations_queue.cpp index 197cf45ae..3c5d09cd5 100644 --- a/panther_lights/src/led_animations_queue.cpp +++ b/panther_lights/src/led_animations_queue.cpp @@ -20,7 +20,8 @@ #include #include -#include +#include +#include namespace panther_lights { @@ -39,6 +40,9 @@ LEDAnimation::LEDAnimation( { for (auto & animation : led_animation_description_.animations) { for (auto & segment : animation.segments) { + if (segments.find(segment) == segments.end()) { + throw std::runtime_error("No segment with name: " + segment); + } animation_segments_.push_back(segments.at(segment)); } } @@ -116,7 +120,10 @@ void LEDAnimationsQueue::Validate(const rclcpp::Time & time) { for (auto it = queue_.begin(); it != queue_.end();) { if ((time - it->get()->GetInitTime()).seconds() > it->get()->GetTimeout()) { - // TODO: log info - animation timeout + RCLCPP_WARN( + rclcpp::get_logger("controller_node"), + "Warning: Timeout for animation: %s. Removing from the queue", + it->get()->GetName().c_str()); it = queue_.erase(it); } else { ++it; diff --git a/panther_lights/test/test_controller_node.cpp b/panther_lights/test/test_controller_node.cpp new file mode 100644 index 000000000..6f42217dd --- /dev/null +++ b/panther_lights/test/test_controller_node.cpp @@ -0,0 +1,325 @@ +// Copyright 2023 Husarion sp. z o.o. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +class ControllerNodeWrapper : public panther_lights::ControllerNode +{ +public: + ControllerNodeWrapper(const std::string & node_name, const rclcpp::NodeOptions & options) + : ControllerNode(node_name, options) + { + } + + void InitializeLEDPanels(const YAML::Node & panels_description) + { + return ControllerNode::InitializeLEDPanels(panels_description); + } + + void InitializeLEDSegments(const YAML::Node & segments_description, const float controller_freq) + { + return ControllerNode::InitializeLEDSegments(segments_description, controller_freq); + } + + void LoadAnimation(const YAML::Node & animation_description) + { + return ControllerNode::LoadAnimation(animation_description); + } + + void AddAnimationToQueue( + const std::size_t animation_id, const bool repeating, const std::string & param = "") + { + return ControllerNode::AddAnimationToQueue(animation_id, repeating, param); + } + + std::shared_ptr GetQueue() { return this->animations_queue_; } + + std::shared_ptr GetCurrentAnimation() + { + return this->current_animation_; + } +}; + +class TestControllerNode : public testing::Test +{ +public: + TestControllerNode(); + ~TestControllerNode(); + +protected: + void CreateLEDConfig(const std::filesystem::path file_path); + void CallSetLEDAnimationSrv( + const std::size_t animaiton_id, const bool repeating, const std::string & param = ""); + + std::filesystem::path led_config_file_; + std::shared_ptr controller_node_; + rclcpp::Client::SharedPtr set_led_anim_client_; +}; + +TestControllerNode::TestControllerNode() +{ + led_config_file_ = testing::TempDir() + "/led_config.yaml"; + + CreateLEDConfig(led_config_file_); + + std::vector params; + params.push_back(rclcpp::Parameter("led_config_file", led_config_file_)); + + rclcpp::NodeOptions options; + options.parameter_overrides(params); + + controller_node_ = std::make_shared("controller_node", options); + + set_led_anim_client_ = controller_node_->create_client( + "lights/controller/set/animation"); +} + +TestControllerNode::~TestControllerNode() { std::filesystem::remove(led_config_file_); } + +void TestControllerNode::CreateLEDConfig(const std::filesystem::path file_path) +{ + YAML::Node panel; + panel["channel"] = 1; + panel["number_of_leds"] = 10; + + YAML::Node segment; + segment["name"] = "test"; + segment["channel"] = 1; + segment["led_range"] = "0-9"; + + YAML::Node segments_map; + segments_map["test"] = std::vector(1, "test"); + + YAML::Node animation; + animation["image"] = "$(find panther_lights)/animations/triangle01_red.png"; + animation["duration"] = 2; + + YAML::Node animation_desc; + animation_desc["type"] = "panther_lights::ImageAnimation"; + animation_desc["segments"] = "test"; + animation_desc["animation"] = animation; + + YAML::Node led_animation_0; + led_animation_0["id"] = 0; + led_animation_0["animations"] = std::vector(1, animation_desc); + YAML::Node led_animation_1; + led_animation_1["id"] = 1; + led_animation_1["animations"] = std::vector(1, animation_desc); + led_animation_1["priority"] = 2; + + std::vector led_animations; + led_animations.push_back(led_animation_0); + led_animations.push_back(led_animation_1); + + YAML::Node led_config; + led_config["panels"] = std::vector(1, panel); + led_config["segments"] = std::vector(1, segment); + led_config["segments_map"] = segments_map; + led_config["led_animations"] = led_animations; + + YAML::Emitter out; + out << led_config; + + std::ofstream fout(file_path); + if (fout.is_open()) { + fout << out.c_str(); + fout.close(); + } else { + throw std::runtime_error("Failed to create file: " + std::string(file_path)); + } +} + +void TestControllerNode::CallSetLEDAnimationSrv( + const std::size_t animation_id, const bool repeating, const std::string & param) +{ + auto request = std::make_shared(); + request->animation.id = animation_id; + request->animation.param = param; + request->repeating = repeating; + + auto result = set_led_anim_client_->async_send_request(request); + + ASSERT_TRUE( + rclcpp::spin_until_future_complete(controller_node_, result) == + rclcpp::FutureReturnCode::SUCCESS); + EXPECT_TRUE(result.get()->success); +} + +TEST(TestControllerNodeInitialization, InitializeLEDPanels) +{ + // ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( + // controller_node_, battery_state_, std::chrono::milliseconds(1000))); +} + +TEST_F(TestControllerNode, InitializeLEDPanelsThrowRepeatingChannel) +{ + YAML::Node panel_1_desc; + panel_1_desc["channel"] = 1; + panel_1_desc["number_of_leds"] = 10; + + YAML::Node panel_2_desc; + panel_2_desc["channel"] = 1; + panel_2_desc["number_of_leds"] = 10; + + std::vector panels; + panels.push_back(panel_1_desc); + panels.push_back(panel_2_desc); + + YAML::Node panels_desc; + panels_desc["panels"] = panels; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->InitializeLEDPanels(panels_desc["panels"]); }, + "Multiple panels with channel nr")); +} + +TEST_F(TestControllerNode, InitializeLEDSegmentsThrowRepeatingName) +{ + YAML::Node segment_1_desc; + segment_1_desc["name"] = "TEST"; + segment_1_desc["channel"] = 1; + segment_1_desc["led_range"] = "0-9"; + + YAML::Node segment_2_desc; + segment_2_desc["name"] = "TEST"; + segment_2_desc["channel"] = 1; + segment_2_desc["led_range"] = "0-9"; + + std::vector segments; + segments.push_back(segment_1_desc); + segments.push_back(segment_2_desc); + + YAML::Node segments_desc; + segments_desc["segments"] = segments; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->InitializeLEDSegments(segments_desc["segments"], 50.0); }, + "Multiple segments with given name found")); +} + +TEST_F(TestControllerNode, LoadAnimationInvalidPriority) +{ + YAML::Node led_animation_desc; + led_animation_desc["id"] = 11; + led_animation_desc["priority"] = 0; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->LoadAnimation(led_animation_desc); }, + "Invalid LED animation priority")); + + led_animation_desc["priority"] = 4; + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->LoadAnimation(led_animation_desc); }, + "Invalid LED animation priority")); +} + +TEST_F(TestControllerNode, LoadAnimationThrowRepeatingID) +{ + YAML::Node led_animation_desc; + led_animation_desc["id"] = 11; + led_animation_desc["animations"] = std::vector(); + + ASSERT_NO_THROW(controller_node_->LoadAnimation(led_animation_desc)); + + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->LoadAnimation(led_animation_desc); }, + "Animation with given ID already exists")); +} + +TEST_F(TestControllerNode, AddAnimationToQueueThrowBadAnimaitonID) +{ + EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( + [&]() { controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:")); +} + +TEST_F(TestControllerNode, AddAnimationToQueue) +{ + auto queue = controller_node_->GetQueue(); + EXPECT_TRUE(queue->Empty()); + EXPECT_NO_THROW(controller_node_->AddAnimationToQueue(0, false)); + EXPECT_FALSE(queue->Empty()); +} + +TEST_F(TestControllerNode, CallSelLEDAnimaitonService) +{ + this->CallSetLEDAnimationSrv(0, false); + + // spin to invoke timer + auto anim = controller_node_->GetCurrentAnimation(); + while (!anim) { + rclcpp::spin_some(controller_node_->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + anim = controller_node_->GetCurrentAnimation(); + } + + EXPECT_STREQ("ANIMATION_0", anim->GetName().c_str()); + + // add another animation to queue + auto queue = controller_node_->GetQueue(); + EXPECT_TRUE(queue->Empty()); + + this->CallSetLEDAnimationSrv(0, false); + + EXPECT_FALSE(queue->Empty()); +} + +TEST_F(TestControllerNode, CallSelLEDAnimaitonServicePriorityInterrupt) +{ + this->CallSetLEDAnimationSrv(0, false); + + // spin to invoke timer + auto anim = controller_node_->GetCurrentAnimation(); + while (!anim) { + rclcpp::spin_some(controller_node_->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + anim = controller_node_->GetCurrentAnimation(); + } + + // add animation with higher priority spin to give time for controller to overwrite current + // animation then check if animation has changed + this->CallSetLEDAnimationSrv(1, false); + for (int i = 0; i < 10; i++) { + rclcpp::spin_some(controller_node_->get_node_base_interface()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + anim = controller_node_->GetCurrentAnimation(); + EXPECT_STREQ("ANIMATION_1", anim->GetName().c_str()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + + auto run_tests = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return run_tests; +} diff --git a/panther_lights/test/test_led_animation.cpp b/panther_lights/test/test_led_animation.cpp index 094b98242..a2985b791 100644 --- a/panther_lights/test/test_led_animation.cpp +++ b/panther_lights/test/test_led_animation.cpp @@ -76,6 +76,21 @@ void TestLEDAnimation::SetSegmentAnimations() } } +TEST(TestLEDAnimationInitialization, InvalidSegmentName) +{ + std::unordered_map> segments; + + panther_lights::AnimationDescription anim_desc; + anim_desc.segments = {"invalid_segment"}; + + panther_lights::LEDAnimationDescription led_anim_desc; + led_anim_desc.animations = {anim_desc}; + + EXPECT_THROW( + std::make_shared(led_anim_desc, segments, rclcpp::Time(0)), + std::runtime_error); +} + TEST_F(TestLEDAnimation, IsFinished) { SetSegmentAnimations(); From 9d0024643adc95388303a29068ce626f35a6da57 Mon Sep 17 00:00:00 2001 From: Dawid Date: Fri, 1 Mar 2024 15:30:41 +0000 Subject: [PATCH 21/32] Update documentation | add launching controller node --- panther_bringup/launch/bringup.launch.py | 24 +++ panther_lights/LIGHTS_API.md | 99 +++++++++++ panther_lights/README.md | 212 +++++++++++++++++++++++ panther_lights/config/led_config.yaml | 123 +++++++++++++ panther_lights/launch/lights.launch.py | 25 ++- 5 files changed, 482 insertions(+), 1 deletion(-) create mode 100644 panther_lights/LIGHTS_API.md create mode 100644 panther_lights/config/led_config.yaml diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 01b20726c..cbc532da6 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -142,6 +142,24 @@ def generate_launch_description(): default_value="", ) + led_config_file = LaunchConfiguration("led_config_file") + declare_led_config_file_arg = DeclareLaunchArgument( + "led_config_file", + default_value=PathJoinSubstitution( + [ + get_package_share_directory("panther_lights"), + "config", + PythonExpression(["'led_config.yaml'"]), + ] + ), + ) + + user_led_animaitons_file = LaunchConfiguration("user_led_animaitons_file") + declare_user_led_animaitons_file_arg = DeclareLaunchArgument( + "user_led_animaitons_file", + default_value="", + ) + simulation_engine = LaunchConfiguration("simulation_engine") declare_simulation_engine_arg = DeclareLaunchArgument( "simulation_engine", @@ -227,6 +245,10 @@ def generate_launch_description(): ) ), condition=UnlessCondition(use_sim), + launch_arguments={ + "led_config_file": led_config_file, + "user_led_animaitons_file": user_led_animaitons_file, + }.items(), ) battery_launch = IncludeLaunchDescription( @@ -283,6 +305,8 @@ def generate_launch_description(): declare_wheel_config_path_arg, declare_controller_config_path_arg, declare_battery_config_path_arg, + declare_led_config_file_arg, + declare_user_led_animaitons_file_arg, declare_simulation_engine_arg, declare_publish_robot_state_arg, declare_use_ekf_arg, diff --git a/panther_lights/LIGHTS_API.md b/panther_lights/LIGHTS_API.md new file mode 100644 index 000000000..777a74c7d --- /dev/null +++ b/panther_lights/LIGHTS_API.md @@ -0,0 +1,99 @@ +# Animation API + +## Animation Class + +Basic animation type. This class consists of: + +Arguments: + +- `animation_description` [*dict*]: a dictionary with animation description. Contain following keys: + - `brightness` [*float*, optional]: will be assigned to the `self._brightness` variable as a value in a range **[0, 255]**. + - `duration` [*float*]: will be assigned to `self._duration` variable. + - `repeat` [*int*, optional]: will be assigned to `self._loops` variable. +- `num_led` [*int*]: number of LEDs in a bumper. +- `controller_frequency` [*float*]: controller frequency **[Hz]** at which animation frames will be processed. + +Functions: + +- `Initialize` +- `Update` - returns a list of length `num_led` with **RGBA** values of colors to be displayed on the Bumper Lights based on the `UpdateFrame` method. Handles looping over animation based on `repeating` parameter, iterating over animation, and updating its progress. +- `UpdateFrame` - returns a list of shape (`num_led`, 4) with **RGBA** values of colors to be displayed on the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. By default, not implemented. +- `Reset` - resets animation to its initial state. If overwritten, it requires calling the parent class implementation first. + +Setters: + +- `SetParam` - allows processing an additional parameter passed to the animation when it is created. + +Getters: + +- `GetBrightness` [*std::uint8_t*]: returns animation brightness. +- `IsFinished` [*bool*]: returns a value indicating if the animation execution process has finished. +- `GetNumberOfLeds` [*std::size_t*]: returns the number of LEDs. +- `GetProgress` [*float*]: returns animation execution progress. +- `GetAnimationLength` [*std::size_t*]: returns animation length. +- `GetAnimationIteration` [*std::size_t*]: returns current animation iteration. + +## Defining a New Animation Type + +It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to improt `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html). + +Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animaiton_description` and processed inside `Initialize` method. See the example below or other animation definitions. + +Create a New Animation Type: + +```c++ +# my_cool_animation.hpp + +#ifndef PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ +#define PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ + +#include + +#include + +class MyCoolAnimation : public Animation +{ +public: + MyCoolAnimation() {} + ~MyCoolAnimation() {} + + void Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) override; + +private: + std::uint8_t value_; +}; + +#endif // PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ +``` + +```c++ +# my_cool_animation.cpp + +#include + +#include + +void MyCoolAnimation::Initialize( + const YAML::Node & animation_description, const std::size_t num_led, + const float controller_frequency) +{ + Animation::Initialize(animation_description, num_led, controller_frequency); +} + +void MyCoolAnimation::SetParam(const std::string & param) +{ + value_ = static_cast(param); +} + +std::vector MyCoolAnimation::UpdateFrame() +{ + return std::vector(this->GetNumberOfLeds() * 4, value_); +} + +#include + +PLUGINLIB_EXPORT_CLASS(my_package::MyCoolAnimation, my_package::Animation) + +``` diff --git a/panther_lights/README.md b/panther_lights/README.md index 5c94cacd8..3728ae5db 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -51,6 +51,218 @@ This node is responsible for displaying frames on the Husarion Panther robot's B - `~global_brightness` [*float*, default: **1.0**]: LED global brightness. The range between **[0.0, 1.0]**. - `~num_led` [*int*, default: **46**]: number of LEDs in a single bumper. +[//]: # (ROS_API_NODE_PARAMETERS_END) +[//]: # (ROS_API_NODE_END) + +[//]: # (ROS_API_NODE_START) +[//]: # (ROS_API_NODE_COMPATIBLE_1_0) +[//]: # (ROS_API_NODE_COMPATIBLE_1_2) +[//]: # (ROS_API_NODE_NAME_START) + +### controller_node + +[//]: # (ROS_API_NODE_NAME_END) +[//]: # (ROS_API_NODE_DESCRIPTION_START) + +This node is responsible for processing animations and publishing frames to be displayed on the Husarion Panther robot Bumper Lights. + +[//]: # (ROS_API_NODE_DESCRIPTION_END) + +#### Publishers + +[//]: # (ROS_API_NODE_PUBLISHERS_START) + +- `/panther/lights/driver/front_panel_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Front Bumper Lights. +- `/panther/lights/driver/rear_panel_frame` [*sensor_msgs/Image*, encoding: **RGBA8**, height: **1**, width: **num_led**]: an animation frame to be displayed on robot Rear Bumper Lights. + +[//]: # (ROS_API_NODE_PUBLISHERS_END) + +#### Service Servers + +[//]: # (ROS_API_NODE_SERVICE_SERVERS_START) + +- `/panther/lights/controller/set/animation` [*panther_msgs/SetLEDAnimation*]: allows setting animation on Bumper Lights based on animation ID. + +[//]: # (ROS_API_NODE_SERVICE_SERVERS_END) + +#### Parameters + +[//]: # (ROS_API_NODE_PARAMETERS_START) + +- `~controller_frequency` [*float*, default: **50.0**]: frequency [Hz] at which the lights controller node will process animations. +- `~led_config_file` [*string*, default: **$(find panther_lights)/panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. +- `~user_led_animaitons_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. + [//]: # (ROS_API_NODE_PARAMETERS_END) [//]: # (ROS_API_NODE_END) [//]: # (ROS_API_PACKAGE_END) + + +## LED configuration + +Basic led configuration is loaded from [`led_config.yaml`](config/led_config.yaml) file. It includes definition of robot panels, virtual segments and default animations. + +### Panels + +The `panels` section of the YAML file lists all the physical LED panels on the robot. Each panel has two attributes: + +- `channel` [*int*, default: **None**] the identifier for the LED panel. It is used to differentiate between multiple panels. +- `number_of_leds`: defines the total number of LEDs present on the panel. + +### Segments + +The `segments` section is used to create virtual segments on the robot by dividing the LED panels into different parts. This allows for more precise control over which LEDs are lit up for different effects or indicators. Each segment has three attributes: + +- `name`: the identifier for the segment, such as "front" or "rear". It is used to differentiate between multiple segments. +- `channel`: This specifies which LED panel the segment belongs to. It have to match one of the channels defined in the `panels` section. +- `led_range`: This defines the range of LEDs within the panel that the segment covers. The range is specified as a start-end pair (e.g. 0-45). The range can be specified in reverse order (e.g. 45-0), which may be useful for wiring or orientation reasons. + +### Segments map + +The `segments_map` section allows creating named groups of segments on which animations can be displayed. Each entry under `segments_map` consists of a key representing the group name and a list of segments included in the group. Segment names have to match one of the segments defined in the `segments` section. By default you can use provided mapping: + +- `all` [*list*, default: **None**]: Grouping both `front` and `rear` segments together. +- `front` [*list*, default: **None**]: Containing only the `front` segment. +- `rear` [*list*, default: **None**]: Containing only the `rear` segment. + +### Animations + +The `led_animations` section contains list with definitions for various animations that can be displayed on the LED segments. Supported keys are: + +- `animations` [*list*, default: **None**]: definition of animation for each Bumper Lights. Supported keys are: + - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `panther_lights::ImageAnimation`, `panther_lights::ChargingAnimation`. + - `segments` [*string*, default **None**]: Indicates which segment mapping this particular animation applies to (e.g., all, front, rear). + - `animaiton` [*yaml*, default: **None**]: An animation to be displayed on segments. The keys for the configuration of different animation types are explained in detail under the [**Animation Types**](#animation-types) section. +- `id` [*int*, default: **None**]: unique ID of an animation. +- `name` [*string*, default: **ANIMATION_**]: name of an animation. If not provided will default to **ANIMATION_**, where `` is equal to `id` parameter of the given animation. +- `priority` [*int*, default: **3**]: priority at which animation will be placed in the queue. The list below shows the behavior when an animation with a given ID arrives: + - **1** interrupts and removes animation with priorities **2** and **3**. + - **2** interrupts animations with priority **3**. + - **3** adds animation to the end of the queue. +- `timeout` [*float*, default: **120.0**]: time in **[s]**, after which animation will be removed from the queue. + +Default animations can be found in the table below: + +| ID | NAME | PRIORITY | DESCRIPTION | +| :-: | ----------------- | :------: | ----------------------------------------------------------- | +| 0 | E_STOP | 3 | red expanding from the center to the edges | +| 1 | READY | 3 | green expanding from center to the edges | +| 2 | ERROR | 1 | red, whole bumper blinking twice | +| 3 | MANUAL_ACTION | 3 | blue expanding from the center to the edges | +| 4 | AUTONOMOUS_ACTION | 3 | orange expanding from center to the edges | +| 5 | GOAL_ACHIEVED | 2 | purple, whole bumper blinking three times | +| 6 | LOW_BATTERY | 2 | two orange stripes moving towards the center, repeats twice | +| 7 | CRITICAL_BATTERY | 2 | two red stripes moving towards the center, repeats twice | +| 9 | CHARGING_BATTERY | 3 | whole bumper blinks with a duty cycle proportional to the Battery percentage. Short blinking means low Battery, and no blinking means full Battery. The color changes from red to green | + +### Animation Types + +#### Animation + +Basic animation definition. Keys are inherited from the basic **Animation** class by all animations. Supported keys are: + +- `brightness` [*float*, default: **1.0**]: animation brightness relative to APA102C driver `global_brightness`. The range between **[0.0, 1.0]**. +- `duration` [*float*, default: **None**]: duration of the animation. +- `repeat` [*int*, default: **1**]: number of times the animation will be repeated. + +> **Note** +> Overall display duration of an animation is a product of a single image duration and repeat count. The result of `duration` x `repeat` can't exceed 10 **[s]**. If animation fails to fulfill the requirement it will result in an error. + +#### ImageAnimation + +Animation of type `panther_lights::ImageAnimation`, returns frames to display based on a supplied image. Extends `Animation` with keys: + +- `color` [*int*, default: **None**]: The image is turned into grayscale, and then the color is applied with brightness from the gray image. Values have to be in HEX format. This parameter is not required. +- `image` [*string*, default: **None**]: path to an image file. Only global paths are valid. Allows using `$(find ros_package)` syntax. + +#### ChargingAnimation + +Animation of type `panther_lights::ChargingAnimation`, returns frame to display based on `param` value representing Battery percentage. Displays a solid color with a duty cycle proportional to the Battery percentage. The color is changing from red (Battery discharged) to green (Battery fully charged). + +### Defining Animations + +Users can define their own LED animations using basic animation types. Similar to basic ones, user animations are parsed using YAML file and loaded on node start. For `ImageAnimation` you can use basic images from the `animations` folder and change their color with the `color` key ([see ImageAnimation](#imageanimation)). Follow the example below to add custom animations. + +Create a YAML file with an animation description list. Example file: + +```yaml +# my_awesome_user_animations.yaml +user_animations: + # animation with default image and custom color + - id: 21 + name: ANIMATION_1 + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_red.png + duration: 2 + repeat: 2 + color: 0xffff00 + + # animation with custom image + - id: 22 + name: ANIMATION_2 + priority: 3 + animation: + - type: image_animation + segments: all + animation: + image: /animations/custom_image.png + duration: 3 + repeat: 1 + + # animation with a custom image from custom ROS package + - id: 23 + name: ANIMATION_3 + priority: 3 + animation: + - type: image_animation + segments: all + animation: + image: $(find my_custom_animation_package)/animations/custom_image.png + duration: 3 + repeat: 1 + + # different animations for Front and Rear Bumper Light + - id: 24 + name: ANIMATION_4 + priority: 3 + animation: + - type: image_animation + segments: front + animation: + image: $(find panther_lights)/animations/triangle01_blue.png + duration: 2 + repeat: 2 + - type: image_animation + segments: rear + animation: + image: $(find panther_lights)/animations/triangle01_red.png + duration: 3 + repeat: 1 +``` + +> **Note** +> ID numbers from 0 to 19 are reserved for system animations. + +> **Note** +> Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**. + +Remember to modify launch command to use user animations: + +``` bash +ros2 launch panther_bringup bringup.launch user_animations_file:=/my_awesome_user_animations.yaml +``` + +Test new animations: + +```bash +ros2 service call /lights/controller/set/animation panther_msgs/srv/SetLEDAnimation "{animation: {id: 0, param: ''}, repeating: true}"" +``` + +--- +## Defining a Custom Animation Type + +It is possible to define your own animation type with expected, new behavior. For more information, see: [**Animation API**](./LIGHTS_API.md). diff --git a/panther_lights/config/led_config.yaml b/panther_lights/config/led_config.yaml new file mode 100644 index 000000000..867342500 --- /dev/null +++ b/panther_lights/config/led_config.yaml @@ -0,0 +1,123 @@ +panels: + - channel: 1 + number_of_leds: 46 + - channel: 2 + number_of_leds: 46 + +segments: + - name: front + channel: 1 + led_range: 0-45 + - name: rear + channel: 2 + led_range: 45-0 + +segments_map: + all: [front, rear] + front: [front] + rear: [rear] + +led_animations: + - id: 0 + name: E_STOP + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_red.png + duration: 2 + + - id: 1 + name: READY + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_green.png + duration: 2 + + - id: 2 + name: ERROR + priority: 1 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_red.png + duration: 3 + repeat: 2 + + - id: 3 + name: MANUAL_ACTION + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_blue.png + duration: 3 + + - id: 4 + name: AUTONOMOUS_ACTION + priority: 3 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/triangle01_orange.png + duration: 3 + + - id: 5 + name: GOAL_ACHIEVED + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/strip01_purple.png + duration: 3 + repeat: 2 + + - id: 6 + name: LOW_BATTERY + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/battery_low.png + duration: 2 + repeat: 2 + + - id: 7 + name: CRITICAL_BATTERY + priority: 2 + animations: + - type: panther_lights::ImageAnimation + segments: all + animation: + image: $(find panther_lights)/animations/battery_critical.png + duration: 2 + repeat: 2 + + # - id: 8 + # name: BATTERY_STATE + # priority: 3 + # animations: + # - type: panther_lights::ImageAnimation + # segments: all + # animation: + # duration: 3 + # repeat: 3 + + - id: 9 + name: CHARGING_BATTERY + priority: 3 + animations: + - type: panther_lights::ChargingAnimation + segments: all + animation: + duration: 3 + repeat: 2 diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py index 988a9cb58..4155c2459 100644 --- a/panther_lights/launch/lights.launch.py +++ b/panther_lights/launch/lights.launch.py @@ -16,11 +16,23 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import Shutdown +from launch.actions import DeclareLaunchArgument, Shutdown +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): + led_config_file = LaunchConfiguration("led_config_file") + declare_led_config_file_arg = DeclareLaunchArgument( + "led_config_file", + ) + + user_led_animaitons_file = LaunchConfiguration("user_led_animaitons_file") + declare_user_led_animaitons_file_arg = DeclareLaunchArgument( + "user_led_animaitons_file", + default_value="", + ) + lights_driver_node = Node( package="panther_lights", executable="driver_node", @@ -28,6 +40,14 @@ def generate_launch_description(): on_exit=Shutdown(), ) + lights_controller_node = Node( + package="panther_lights", + executable="controller_node", + name="lights_controller_node", + parameters=[led_config_file, user_led_animaitons_file], + on_exit=Shutdown(), + ) + dummy_scheduler_node = Node( package="panther_lights", executable="dummy_scheduler_node", @@ -35,7 +55,10 @@ def generate_launch_description(): ) actions = [ + declare_led_config_file_arg, + declare_user_led_animaitons_file_arg, lights_driver_node, + lights_controller_node, dummy_scheduler_node, ] From 38ef09a9f19c4f0abc29936e67e73d0b30230e67 Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 5 Mar 2024 10:45:53 +0000 Subject: [PATCH 22/32] make it work --- panther_lights/launch/lights.launch.py | 5 ++++- panther_lights/src/driver_node.cpp | 5 +++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py index 4155c2459..cbe17328a 100644 --- a/panther_lights/launch/lights.launch.py +++ b/panther_lights/launch/lights.launch.py @@ -44,7 +44,10 @@ def generate_launch_description(): package="panther_lights", executable="controller_node", name="lights_controller_node", - parameters=[led_config_file, user_led_animaitons_file], + parameters=[ + {"led_config_file": led_config_file}, + {"user_led_animaitons_file": user_led_animaitons_file}, + ], on_exit=Shutdown(), ) diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 14f2e4bbc..988504c48 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -68,6 +68,7 @@ void DriverNode::Initialize() std::vector gpio_info_storage = {panther_gpiod::GPIOInfo{ panther_gpiod::GPIOPin::LED_SBC_SEL, gpiod::line::direction::OUTPUT, true}}; gpio_driver_ = std::make_unique(gpio_info_storage); + gpio_driver_->GPIOMonitorEnable(); front_panel_ts_ = this->get_clock()->now(); rear_panel_ts_ = this->get_clock()->now(); @@ -76,13 +77,13 @@ void DriverNode::Initialize() rear_panel_.SetGlobalBrightness(global_brightness); front_light_sub_ = std::make_shared( - it_->subscribe("lights/driver/front_panel_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { + it_->subscribe("lights/driver/channel_1_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { FrameCB(msg, front_panel_, front_panel_ts_, "front"); front_panel_ts_ = msg->header.stamp; })); rear_light_sub_ = std::make_shared( - it_->subscribe("lights/driver/rear_panel_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { + it_->subscribe("lights/driver/channel_2_frame", 5, [&](const ImageMsg::ConstSharedPtr & msg) { FrameCB(msg, rear_panel_, rear_panel_ts_, "rear"); rear_panel_ts_ = msg->header.stamp; })); From a3b9858c495b4ef2b05e313b978c5b16fe2ad39c Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 5 Mar 2024 10:46:26 +0000 Subject: [PATCH 23/32] update scheduler --- .../panther_lights/dummy_scheduler_node.hpp | 24 ++------ panther_lights/src/dummy_scheduler_node.cpp | 57 +++++++------------ 2 files changed, 27 insertions(+), 54 deletions(-) diff --git a/panther_lights/include/panther_lights/dummy_scheduler_node.hpp b/panther_lights/include/panther_lights/dummy_scheduler_node.hpp index 2a420da96..644755f9f 100644 --- a/panther_lights/include/panther_lights/dummy_scheduler_node.hpp +++ b/panther_lights/include/panther_lights/dummy_scheduler_node.hpp @@ -25,19 +25,14 @@ #include #include +#include + namespace panther_lights { using BoolMsg = std_msgs::msg::Bool; using BatteryStateMsg = sensor_msgs::msg::BatteryState; - -struct RGBAColor -{ - std::uint8_t r; - std::uint8_t g; - std::uint8_t b; - std::uint8_t a; -}; +using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; class SchedulerNode : public rclcpp::Node { @@ -45,27 +40,20 @@ class SchedulerNode : public rclcpp::Node SchedulerNode( const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - void Initialize(); - private: void ControllerTimerCB(); - void PublishColor(const RGBAColor & color); - - static constexpr RGBAColor kColorRed = {255, 0, 0, 255}; - static constexpr RGBAColor kColorGreen = {0, 255, 0, 255}; - static constexpr RGBAColor kColorOrange = {255, 140, 0, 255}; + void CallSetLEDAnimationSrv(const std::uint16_t animation_id); bool e_stop_state_ = true; int num_led_; float battery_percentage_; + std::uint16_t current_anim_id_; rclcpp::TimerBase::SharedPtr controller_timer_; rclcpp::Subscription::SharedPtr e_stop_sub_; rclcpp::Subscription::SharedPtr battery_state_sub_; - std::shared_ptr it_; - std::shared_ptr rear_light_pub_; - std::shared_ptr front_light_pub_; + rclcpp::Client::SharedPtr set_animation_client_; }; } // namespace panther_lights diff --git a/panther_lights/src/dummy_scheduler_node.cpp b/panther_lights/src/dummy_scheduler_node.cpp index 6b86c8e0d..5fac6bf81 100644 --- a/panther_lights/src/dummy_scheduler_node.cpp +++ b/panther_lights/src/dummy_scheduler_node.cpp @@ -23,6 +23,8 @@ #include #include +#include + namespace panther_lights { @@ -35,12 +37,8 @@ SchedulerNode::SchedulerNode(const std::string & node_name, const rclcpp::NodeOp controller_timer_ = this->create_wall_timer( std::chrono::milliseconds(50), std::bind(&SchedulerNode::ControllerTimerCB, this)); - RCLCPP_INFO(this->get_logger(), "Node started"); -} - -void SchedulerNode::Initialize() -{ - it_ = std::make_shared(this->shared_from_this()); + set_animation_client_ = + this->create_client("lights/controller/set/animation"); e_stop_sub_ = this->create_subscription( "hardware/e_stop", 5, [&](const BoolMsg::SharedPtr msg) { e_stop_state_ = msg->data; }); @@ -48,50 +46,37 @@ void SchedulerNode::Initialize() "battery", 5, [&](const BatteryStateMsg::SharedPtr msg) { battery_percentage_ = msg->percentage; }); - front_light_pub_ = std::make_shared( - it_->advertise("lights/driver/front_panel_frame", 5)); - rear_light_pub_ = std::make_shared( - it_->advertise("lights/driver/rear_panel_frame", 5)); - - RCLCPP_INFO(this->get_logger(), "Controller initialised"); + RCLCPP_INFO(this->get_logger(), "Node started"); } void SchedulerNode::ControllerTimerCB() { - if (!it_) { - Initialize(); - } - if (battery_percentage_ < 0.4) { - PublishColor(kColorOrange); + CallSetLEDAnimationSrv(6); } else if (!e_stop_state_) { - PublishColor(kColorGreen); + CallSetLEDAnimationSrv(1); } else { - PublishColor(kColorRed); + CallSetLEDAnimationSrv(0); } } -void SchedulerNode::PublishColor(const RGBAColor & color) +void SchedulerNode::CallSetLEDAnimationSrv(const std::uint16_t animation_id) { - sensor_msgs::msg::Image image_msg; - image_msg.header.stamp = this->get_clock()->now(); - image_msg.encoding = sensor_msgs::image_encodings::RGBA8; - image_msg.height = 1; - image_msg.width = num_led_; - image_msg.step = image_msg.width * 4; // 4 for RGBA channels - - for (int i = 0; i < num_led_; i++) { - image_msg.data.push_back(color.r); - image_msg.data.push_back(color.g); - image_msg.data.push_back(color.b); - image_msg.data.push_back(color.a); + if (current_anim_id_ == animation_id) { + return; } - image_msg.header.frame_id = "front_light_link"; - front_light_pub_->publish(image_msg); + RCLCPP_INFO(this->get_logger(), "Calling LED srv"); + if (!set_animation_client_->wait_for_service(std::chrono::milliseconds(1000))) { + RCLCPP_INFO(this->get_logger(), "Service not available"); + return; + } + auto req = std::make_shared(); + req->animation.id = animation_id; + req->repeating = true; + set_animation_client_->async_send_request(req); - image_msg.header.frame_id = "rear_light_link"; - rear_light_pub_->publish(image_msg); + current_anim_id_ = animation_id; } } // namespace panther_lights From 628a3294b4bd88e9a618d1375438c1c9d05bce81 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Tue, 5 Mar 2024 14:21:09 +0100 Subject: [PATCH 24/32] Update panther_lights/LIGHTS_API.md MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Paweł Irzyk <108666440+pawelirh@users.noreply.github.com> --- panther_lights/LIGHTS_API.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/LIGHTS_API.md b/panther_lights/LIGHTS_API.md index 777a74c7d..380f2ee61 100644 --- a/panther_lights/LIGHTS_API.md +++ b/panther_lights/LIGHTS_API.md @@ -35,7 +35,7 @@ Getters: ## Defining a New Animation Type -It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to improt `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html). +It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html). Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animaiton_description` and processed inside `Initialize` method. See the example below or other animation definitions. From 0c8f5df1577ae0912cbc60578d8471a285154252 Mon Sep 17 00:00:00 2001 From: Dawid Date: Tue, 5 Mar 2024 14:44:48 +0000 Subject: [PATCH 25/32] review fixes --- panther_bringup/README.md | 2 + panther_bringup/launch/bringup.launch.py | 2 + panther_lights/LIGHTS_API.md | 14 +++--- panther_lights/README.md | 8 +-- panther_lights/config/led_config.yaml | 11 +---- .../panther_lights/controller_node.hpp | 3 -- .../panther_lights/led_animations_queue.hpp | 8 +++ panther_lights/launch/lights.launch.py | 2 + panther_lights/src/controller_node.cpp | 13 ++--- panther_lights/src/led_animations_queue.cpp | 9 ++-- panther_lights/src/segment_converter.cpp | 2 +- panther_lights/test/test_controller_node.cpp | 43 ++++++++-------- panther_lights/test/test_led_animation.cpp | 49 ++++++++++--------- .../test/test_led_animations_queue.cpp | 18 ------- panther_lights/test/test_led_segment.cpp | 3 +- panther_utils/CMakeLists.txt | 1 + .../include/panther_utils/yaml_utils.hpp | 2 +- panther_utils/test/test_yaml_utils.cpp | 2 +- 18 files changed, 87 insertions(+), 105 deletions(-) diff --git a/panther_bringup/README.md b/panther_bringup/README.md index d61031d4c..ffd505eea 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -21,10 +21,12 @@ The package contains the default configuration and launch files necessary to sta - `battery_config_path` [*string*, default: **None**]: path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. - `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: path to controller configuration file. A path to custom configuration can be specified here. - `ekf_config_path` [*string*, default: **panther_bringup/config/ekf.yaml**]: path to the EKF configuration file. +- `led_config_file` [*string*, default: **panther_lighst/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. - `publish_robot_state` [*bool*, default: **true**]: whether to publish the default Panther robot description. - `simulation_engine` [*string*, default: **ignition-gazebo**]: simulation engine to use when running Gazebo. - `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter. - `use_sim` [*bool*, default: **false**]: whether simulation is used. +- `user_led_animaitons_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. - `wheel_config_path` [*string*, default: **$(find panther_description)/config/.yaml**]: path to YAML file with wheel specification. Arguments become required if `wheel_type` is set to **custom**. - `wheel_type` [*string*, default: **WH01**]: type of wheel, possible are: **WH01** - offroad, **WH02** - mecanum, **WH04** - small pneumatic, and **custom** - custom wheel types (requires setting `wheel_config_path` argument accordingly). diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 9dee284ee..9fea086e4 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -151,12 +151,14 @@ def generate_launch_description(): PythonExpression(["'led_config.yaml'"]), ] ), + description="Path to a YAML file with a description of led configuration", ) user_led_animaitons_file = LaunchConfiguration("user_led_animaitons_file") declare_user_led_animaitons_file_arg = DeclareLaunchArgument( "user_led_animaitons_file", default_value="", + description="Path to a YAML file with a description of the user defined animations", ) simulation_engine = LaunchConfiguration("simulation_engine") diff --git a/panther_lights/LIGHTS_API.md b/panther_lights/LIGHTS_API.md index 777a74c7d..7b96a6ea4 100644 --- a/panther_lights/LIGHTS_API.md +++ b/panther_lights/LIGHTS_API.md @@ -7,15 +7,15 @@ Basic animation type. This class consists of: Arguments: - `animation_description` [*dict*]: a dictionary with animation description. Contain following keys: - - `brightness` [*float*, optional]: will be assigned to the `self._brightness` variable as a value in a range **[0, 255]**. - - `duration` [*float*]: will be assigned to `self._duration` variable. - - `repeat` [*int*, optional]: will be assigned to `self._loops` variable. + - `brightness` [*float*, optional]: will be assigned to the `brightness_` variable as a value in a range **[0, 255]**. + - `duration` [*float*]: will be assigned to `duration_` variable. + - `repeat` [*int*, optional]: will be assigned to `loops_` variable. - `num_led` [*int*]: number of LEDs in a bumper. - `controller_frequency` [*float*]: controller frequency **[Hz]** at which animation frames will be processed. Functions: -- `Initialize` +- `Initialize` - this method handles animation initialization. It should be invoked right after the animation is created, as the constructors for pluginlib classes cannot have parameters. - `Update` - returns a list of length `num_led` with **RGBA** values of colors to be displayed on the Bumper Lights based on the `UpdateFrame` method. Handles looping over animation based on `repeating` parameter, iterating over animation, and updating its progress. - `UpdateFrame` - returns a list of shape (`num_led`, 4) with **RGBA** values of colors to be displayed on the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. By default, not implemented. - `Reset` - resets animation to its initial state. If overwritten, it requires calling the parent class implementation first. @@ -44,8 +44,8 @@ Create a New Animation Type: ```c++ # my_cool_animation.hpp -#ifndef PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ -#define PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ +#ifndef MY_PACKAGE_MY_COOL_ANIMATION_HPP_ +#define MY_PACKAGE_MY_COOL_ANIMATION_HPP_ #include @@ -65,7 +65,7 @@ private: std::uint8_t value_; }; -#endif // PANTHER_LIGHTS_CHARGING_ANIMATION_HPP_ +#endif // MY_PACKAGE_MY_COOL_ANIMATION_HPP_ ``` ```c++ diff --git a/panther_lights/README.md b/panther_lights/README.md index b36e9a5bc..e5f4cfb18 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -214,7 +214,7 @@ user_animations: name: ANIMATION_2 priority: 3 animation: - - type: image_animation + - type: panther_lights::ImageAnimation segments: all animation: image: /animations/custom_image.png @@ -226,7 +226,7 @@ user_animations: name: ANIMATION_3 priority: 3 animation: - - type: image_animation + - type: panther_lights::ImageAnimation segments: all animation: image: $(find my_custom_animation_package)/animations/custom_image.png @@ -238,13 +238,13 @@ user_animations: name: ANIMATION_4 priority: 3 animation: - - type: image_animation + - type: panther_lights::ImageAnimation segments: front animation: image: $(find panther_lights)/animations/triangle01_blue.png duration: 2 repeat: 2 - - type: image_animation + - type: panther_lights::ImageAnimation segments: rear animation: image: $(find panther_lights)/animations/triangle01_red.png diff --git a/panther_lights/config/led_config.yaml b/panther_lights/config/led_config.yaml index 867342500..4860ad504 100644 --- a/panther_lights/config/led_config.yaml +++ b/panther_lights/config/led_config.yaml @@ -102,15 +102,8 @@ led_animations: duration: 2 repeat: 2 - # - id: 8 - # name: BATTERY_STATE - # priority: 3 - # animations: - # - type: panther_lights::ImageAnimation - # segments: all - # animation: - # duration: 3 - # repeat: 3 + # The BATTERY_STATE animation is no longer supported. + # Animation ID: 8 was intentionally omitted to keep numbering compatibility with ROS 1. - id: 9 name: CHARGING_BATTERY diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 629360d9d..e4dfe66bc 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -16,7 +16,6 @@ #define PANTHER_LIGHTS_CONTROLLER_NODE_HPP_ #include -#include #include #include @@ -162,8 +161,6 @@ class ControllerNode : public rclcpp::Node rclcpp::Service::SharedPtr set_led_animation_server_; rclcpp::TimerBase::SharedPtr controller_timer_; - std::mutex queue_mtx_; - bool animation_finished_ = true; }; diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp index 2ff0a440a..73dd97bd4 100644 --- a/panther_lights/include/panther_lights/led_animations_queue.hpp +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -32,6 +32,10 @@ namespace panther_lights { +/** + * @brief Structure describing basic animation, including its type, description and list of segments + * it will be assigned to + */ struct AnimationDescription { std::string type; @@ -39,6 +43,10 @@ struct AnimationDescription YAML::Node animation; }; +/** + * @brief Structure describing a complete LED animation, containing its ID, priority, name, timeout + * and, a list of animations that will be displayed on LED segments + */ struct LEDAnimationDescription { std::uint8_t id; diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py index cbe17328a..9e72f0516 100644 --- a/panther_lights/launch/lights.launch.py +++ b/panther_lights/launch/lights.launch.py @@ -25,12 +25,14 @@ def generate_launch_description(): led_config_file = LaunchConfiguration("led_config_file") declare_led_config_file_arg = DeclareLaunchArgument( "led_config_file", + description="Path to a YAML file with a description of led configuration", ) user_led_animaitons_file = LaunchConfiguration("user_led_animaitons_file") declare_user_led_animaitons_file_arg = DeclareLaunchArgument( "user_led_animaitons_file", default_value="", + description="Path to a YAML file with a description of the user defined animations", ) lights_driver_node = Node( diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 7812c83fc..9e37f74dd 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -40,16 +40,13 @@ namespace panther_lights { -using std::placeholders::_1; -using std::placeholders::_2; - ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { - // TODO, move default led_config_file to launch - this->declare_parameter( - "led_config_file", - "/home/husarion/ros2_ws/src/panther_ros/panther_lights/config/led_config.yaml"); + using std::placeholders::_1; + using std::placeholders::_2; + + this->declare_parameter("led_config_file"); this->declare_parameter("user_led_animaitons_file", ""); this->declare_parameter("controller_freq", 50.0); @@ -266,8 +263,6 @@ void ControllerNode::PublishPanelFrame(const std::size_t channel) void ControllerNode::ControllerTimerCB() { - std::lock_guard lck_g(queue_mtx_); - if (animation_finished_) { animations_queue_->Validate(this->get_clock()->now()); diff --git a/panther_lights/src/led_animations_queue.cpp b/panther_lights/src/led_animations_queue.cpp index 3c5d09cd5..65c4f5e4e 100644 --- a/panther_lights/src/led_animations_queue.cpp +++ b/panther_lights/src/led_animations_queue.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -26,9 +26,6 @@ namespace panther_lights { -using std::placeholders::_1; -using std::placeholders::_2; - LEDAnimation::LEDAnimation( const LEDAnimationDescription & led_animation_description, const std::unordered_map> & segments, @@ -38,8 +35,8 @@ LEDAnimation::LEDAnimation( repeating_(false), param_("") { - for (auto & animation : led_animation_description_.animations) { - for (auto & segment : animation.segments) { + for (const auto & animation : led_animation_description_.animations) { + for (const auto & segment : animation.segments) { if (segments.find(segment) == segments.end()) { throw std::runtime_error("No segment with name: " + segment); } diff --git a/panther_lights/src/segment_converter.cpp b/panther_lights/src/segment_converter.cpp index d8ace49e7..c6594f89d 100644 --- a/panther_lights/src/segment_converter.cpp +++ b/panther_lights/src/segment_converter.cpp @@ -30,7 +30,7 @@ void SegmentConverter::Convert( const std::unordered_map> & segments, const std::unordered_map> & panels) { - for (auto & [segment_name, segment] : segments) { + for (const auto & [segment_name, segment] : segments) { if (!segment->HasAnimation()) { continue; } diff --git a/panther_lights/test/test_controller_node.cpp b/panther_lights/test/test_controller_node.cpp index 6f42217dd..e253c701e 100644 --- a/panther_lights/test/test_controller_node.cpp +++ b/panther_lights/test/test_controller_node.cpp @@ -76,6 +76,11 @@ class TestControllerNode : public testing::Test void CallSetLEDAnimationSrv( const std::size_t animaiton_id, const bool repeating, const std::string & param = ""); + static constexpr std::size_t kTestNumberOfLeds = 10; + static constexpr std::size_t kTestChannel = 1; + static constexpr char kTestSegmentName[] = "test"; + static constexpr char kTestSegmentLedRange[] = "0-9"; + std::filesystem::path led_config_file_; std::shared_ptr controller_node_; rclcpp::Client::SharedPtr set_led_anim_client_; @@ -104,16 +109,16 @@ TestControllerNode::~TestControllerNode() { std::filesystem::remove(led_config_f void TestControllerNode::CreateLEDConfig(const std::filesystem::path file_path) { YAML::Node panel; - panel["channel"] = 1; - panel["number_of_leds"] = 10; + panel["channel"] = kTestChannel; + panel["number_of_leds"] = kTestNumberOfLeds; YAML::Node segment; - segment["name"] = "test"; - segment["channel"] = 1; - segment["led_range"] = "0-9"; + segment["name"] = kTestSegmentName; + segment["channel"] = kTestChannel; + segment["led_range"] = kTestSegmentLedRange; YAML::Node segments_map; - segments_map["test"] = std::vector(1, "test"); + segments_map["test"] = std::vector(1, kTestSegmentName); YAML::Node animation; animation["image"] = "$(find panther_lights)/animations/triangle01_red.png"; @@ -170,21 +175,15 @@ void TestControllerNode::CallSetLEDAnimationSrv( EXPECT_TRUE(result.get()->success); } -TEST(TestControllerNodeInitialization, InitializeLEDPanels) -{ - // ASSERT_TRUE(panther_utils::test_utils::WaitForMsg( - // controller_node_, battery_state_, std::chrono::milliseconds(1000))); -} - TEST_F(TestControllerNode, InitializeLEDPanelsThrowRepeatingChannel) { YAML::Node panel_1_desc; - panel_1_desc["channel"] = 1; - panel_1_desc["number_of_leds"] = 10; + panel_1_desc["channel"] = kTestChannel; + panel_1_desc["number_of_leds"] = kTestNumberOfLeds; YAML::Node panel_2_desc; - panel_2_desc["channel"] = 1; - panel_2_desc["number_of_leds"] = 10; + panel_2_desc["channel"] = kTestChannel; + panel_2_desc["number_of_leds"] = kTestNumberOfLeds; std::vector panels; panels.push_back(panel_1_desc); @@ -201,14 +200,14 @@ TEST_F(TestControllerNode, InitializeLEDPanelsThrowRepeatingChannel) TEST_F(TestControllerNode, InitializeLEDSegmentsThrowRepeatingName) { YAML::Node segment_1_desc; - segment_1_desc["name"] = "TEST"; - segment_1_desc["channel"] = 1; - segment_1_desc["led_range"] = "0-9"; + segment_1_desc["name"] = kTestSegmentName; + segment_1_desc["channel"] = kTestChannel; + segment_1_desc["led_range"] = kTestSegmentLedRange; YAML::Node segment_2_desc; - segment_2_desc["name"] = "TEST"; - segment_2_desc["channel"] = 1; - segment_2_desc["led_range"] = "0-9"; + segment_2_desc["name"] = kTestSegmentName; + segment_2_desc["channel"] = kTestChannel; + segment_2_desc["led_range"] = kTestSegmentLedRange; std::vector segments; segments.push_back(segment_1_desc); diff --git a/panther_lights/test/test_led_animation.cpp b/panther_lights/test/test_led_animation.cpp index a2985b791..79273d5b9 100644 --- a/panther_lights/test/test_led_animation.cpp +++ b/panther_lights/test/test_led_animation.cpp @@ -35,6 +35,9 @@ class TestLEDAnimation : public testing::Test void SetSegmentAnimations(); protected: + static constexpr char kTestSegmentName1[] = "segment_1"; + static constexpr char kTestSegmentName2[] = "segment_2"; + std::shared_ptr led_anim_; std::unordered_map> segments_; }; @@ -44,12 +47,12 @@ TestLEDAnimation::TestLEDAnimation() auto segment_1_desc = YAML::Load("{channel: 1, led_range: 0-10}"); auto segment_2_desc = YAML::Load("{channel: 2, led_range: 0-10}"); segments_.emplace( - "segment_1", std::make_shared(segment_1_desc, 50.0)); + kTestSegmentName1, std::make_shared(segment_1_desc, 50.0)); segments_.emplace( - "segment_2", std::make_shared(segment_2_desc, 50.0)); + kTestSegmentName2, std::make_shared(segment_2_desc, 50.0)); panther_lights::AnimationDescription anim_desc; - anim_desc.segments = {"segment_1", "segment_2"}; + anim_desc.segments = {kTestSegmentName1, kTestSegmentName2}; anim_desc.type = "panther_lights::ImageAnimation"; anim_desc.animation = YAML::Load("{image: $(find panther_lights)/animations/triangle01_red.png, duration: 2.0}"); @@ -97,14 +100,14 @@ TEST_F(TestLEDAnimation, IsFinished) EXPECT_FALSE(led_anim_->IsFinished()); - while (!segments_.at("segment_1")->IsAnimationFinished()) { - segments_.at("segment_1")->UpdateAnimation(); + while (!segments_.at(kTestSegmentName1)->IsAnimationFinished()) { + segments_.at(kTestSegmentName1)->UpdateAnimation(); } EXPECT_FALSE(led_anim_->IsFinished()); - while (!segments_.at("segment_2")->IsAnimationFinished()) { - segments_.at("segment_2")->UpdateAnimation(); + while (!segments_.at(kTestSegmentName2)->IsAnimationFinished()) { + segments_.at(kTestSegmentName2)->UpdateAnimation(); } EXPECT_TRUE(led_anim_->IsFinished()); @@ -116,15 +119,15 @@ TEST_F(TestLEDAnimation, GetProgress) EXPECT_FLOAT_EQ(0.0, led_anim_->GetProgress()); - while (!segments_.at("segment_1")->IsAnimationFinished()) { - segments_.at("segment_1")->UpdateAnimation(); + while (!segments_.at(kTestSegmentName1)->IsAnimationFinished()) { + segments_.at(kTestSegmentName1)->UpdateAnimation(); } EXPECT_FLOAT_EQ(0.0, led_anim_->GetProgress()); EXPECT_FALSE(led_anim_->IsFinished()); - while (!segments_.at("segment_2")->IsAnimationFinished()) { - segments_.at("segment_2")->UpdateAnimation(); + while (!segments_.at(kTestSegmentName2)->IsAnimationFinished()) { + segments_.at(kTestSegmentName2)->UpdateAnimation(); } EXPECT_FLOAT_EQ(1.0, led_anim_->GetProgress()); @@ -134,30 +137,30 @@ TEST_F(TestLEDAnimation, Reset) { SetSegmentAnimations(); - while (!segments_.at("segment_1")->IsAnimationFinished()) { - segments_.at("segment_1")->UpdateAnimation(); + while (!segments_.at(kTestSegmentName1)->IsAnimationFinished()) { + segments_.at(kTestSegmentName1)->UpdateAnimation(); } - while (!segments_.at("segment_2")->IsAnimationFinished()) { - segments_.at("segment_2")->UpdateAnimation(); + while (!segments_.at(kTestSegmentName2)->IsAnimationFinished()) { + segments_.at(kTestSegmentName2)->UpdateAnimation(); } EXPECT_TRUE(led_anim_->GetInitTime() == rclcpp::Time(0)); EXPECT_TRUE(led_anim_->IsFinished()); - EXPECT_TRUE(segments_.at("segment_1")->IsAnimationFinished()); - EXPECT_TRUE(segments_.at("segment_2")->IsAnimationFinished()); - EXPECT_FLOAT_EQ(1.0, segments_.at("segment_1")->GetAnimationProgress()); - EXPECT_FLOAT_EQ(1.0, segments_.at("segment_2")->GetAnimationProgress()); + EXPECT_TRUE(segments_.at(kTestSegmentName1)->IsAnimationFinished()); + EXPECT_TRUE(segments_.at(kTestSegmentName2)->IsAnimationFinished()); + EXPECT_FLOAT_EQ(1.0, segments_.at(kTestSegmentName1)->GetAnimationProgress()); + EXPECT_FLOAT_EQ(1.0, segments_.at(kTestSegmentName2)->GetAnimationProgress()); auto reset_time = rclcpp::Time(1); led_anim_->Reset(reset_time); EXPECT_TRUE(led_anim_->GetInitTime() == reset_time); EXPECT_FALSE(led_anim_->IsFinished()); - EXPECT_FALSE(segments_.at("segment_1")->IsAnimationFinished()); - EXPECT_FALSE(segments_.at("segment_2")->IsAnimationFinished()); - EXPECT_FLOAT_EQ(0.0, segments_.at("segment_1")->GetAnimationProgress()); - EXPECT_FLOAT_EQ(0.0, segments_.at("segment_2")->GetAnimationProgress()); + EXPECT_FALSE(segments_.at(kTestSegmentName1)->IsAnimationFinished()); + EXPECT_FALSE(segments_.at(kTestSegmentName2)->IsAnimationFinished()); + EXPECT_FLOAT_EQ(0.0, segments_.at(kTestSegmentName1)->GetAnimationProgress()); + EXPECT_FLOAT_EQ(0.0, segments_.at(kTestSegmentName2)->GetAnimationProgress()); } int main(int argc, char ** argv) diff --git a/panther_lights/test/test_led_animations_queue.cpp b/panther_lights/test/test_led_animations_queue.cpp index 8ce89ce40..c673bc742 100644 --- a/panther_lights/test/test_led_animations_queue.cpp +++ b/panther_lights/test/test_led_animations_queue.cpp @@ -76,8 +76,6 @@ panther_lights::LEDAnimation TestLEDAnimationsQueue::CreateLEDAnimation( TEST_F(TestLEDAnimationsQueue, Put) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); led_anim_queue_->Put(led_anim, rclcpp::Time(0)); @@ -87,8 +85,6 @@ TEST_F(TestLEDAnimationsQueue, Put) TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1)); for (std::size_t i = 0; i < max_queue_size_; i++) { led_anim_queue_->Put(led_anim, rclcpp::Time(0)); @@ -99,8 +95,6 @@ TEST_F(TestLEDAnimationsQueue, PutQueueOverloaded) TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim_pr_1 = std::make_shared(CreateLEDAnimation("TEST", 1)); auto led_anim_pr_2 = @@ -119,8 +113,6 @@ TEST_F(TestLEDAnimationsQueue, PutClearWhenPriorityEqualOne) TEST_F(TestLEDAnimationsQueue, PutSortByPriority) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim_pr_2 = std::make_shared(CreateLEDAnimation("TEST", 2)); auto led_anim_pr_3 = @@ -139,8 +131,6 @@ TEST_F(TestLEDAnimationsQueue, PutSortByPriority) TEST_F(TestLEDAnimationsQueue, PutSortByTime) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim_t0 = std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); auto led_anim_t1 = @@ -168,8 +158,6 @@ TEST_F(TestLEDAnimationsQueue, GetQueueEmpty) TEST_F(TestLEDAnimationsQueue, Clear) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim_pr_1 = std::make_shared(CreateLEDAnimation("TEST1", 1)); auto led_anim_pr_2 = @@ -197,8 +185,6 @@ TEST_F(TestLEDAnimationsQueue, Clear) TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim = std::make_shared(CreateLEDAnimation("TEST", 1, rclcpp::Time(0))); @@ -214,8 +200,6 @@ TEST_F(TestLEDAnimationsQueue, ValidateAnimationTimedOut) TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim_pr_1 = std::make_shared(CreateLEDAnimation("TEST1", 1)); auto led_anim_pr_2 = @@ -239,8 +223,6 @@ TEST_F(TestLEDAnimationsQueue, GetFirstAnimationPriority) TEST_F(TestLEDAnimationsQueue, Remove) { - EXPECT_TRUE(led_anim_queue_->Empty()); - auto led_anim_pr_1 = std::make_shared(CreateLEDAnimation("TEST1", 1)); auto led_anim_pr_2 = diff --git a/panther_lights/test/test_led_segment.cpp b/panther_lights/test/test_led_segment.cpp index dec99a6f2..0afe24370 100644 --- a/panther_lights/test/test_led_segment.cpp +++ b/panther_lights/test/test_led_segment.cpp @@ -191,10 +191,11 @@ TEST_F(TestLEDSegment, SetAnimation) const auto image_anim_desc = YAML::Load( "{image: $(find panther_lights)/animations/triangle01_red.png, " "duration: 2}"); + const auto charging_anim_desc = YAML::Load("{duration: 2}"); + EXPECT_NO_THROW( led_segment_->SetAnimation("panther_lights::ImageAnimation", image_anim_desc, false)); - const auto charging_anim_desc = YAML::Load("{duration: 2}"); EXPECT_NO_THROW(led_segment_->SetAnimation( "panther_lights::ChargingAnimation", charging_anim_desc, false, "0.5")); } diff --git a/panther_utils/CMakeLists.txt b/panther_utils/CMakeLists.txt index ebf5449fe..2197c69e0 100644 --- a/panther_utils/CMakeLists.txt +++ b/panther_utils/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(diagnostic_updater REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(realtime_tools REQUIRED) +find_package(yaml-cpp REQUIRED) include_directories(${GTEST_INCLUDE_DIRS}) diff --git a/panther_utils/include/panther_utils/yaml_utils.hpp b/panther_utils/include/panther_utils/yaml_utils.hpp index 419050ac5..7e9f73a5e 100644 --- a/panther_utils/include/panther_utils/yaml_utils.hpp +++ b/panther_utils/include/panther_utils/yaml_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/panther_utils/test/test_yaml_utils.cpp b/panther_utils/test/test_yaml_utils.cpp index 2f93af0b9..cb3374236 100644 --- a/panther_utils/test/test_yaml_utils.cpp +++ b/panther_utils/test/test_yaml_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Husarion sp. z o.o. +// Copyright 2024 Husarion sp. z o.o. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From 00295938d7d28aa02dd2f363f307310900c62695 Mon Sep 17 00:00:00 2001 From: Dawid Date: Wed, 6 Mar 2024 07:45:01 +0000 Subject: [PATCH 26/32] update pre-commit and fix typos --- .pre-commit-config.yaml | 2 +- panther_bringup/README.md | 2 +- panther_bringup/launch/bringup.launch.py | 10 +++++----- panther_lights/LIGHTS_API.md | 2 +- panther_lights/README.md | 4 ++-- .../panther_lights/controller_node.hpp | 2 +- .../panther_lights/led_animations_queue.hpp | 6 +++--- .../include/panther_lights/led_segment.hpp | 2 +- panther_lights/launch/lights.launch.py | 10 +++++----- panther_lights/src/controller_node.cpp | 8 ++++---- panther_lights/src/driver_node.cpp | 20 +++++++++---------- panther_lights/test/test_animation.cpp | 6 +++--- panther_lights/test/test_controller_node.cpp | 8 ++++---- 13 files changed, 41 insertions(+), 41 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6f979246b..250258515 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -39,7 +39,7 @@ repos: - id: clang-format - repo: https://github.com/codespell-project/codespell - rev: v1.16.0 + rev: v2.2.6 hooks: - id: codespell name: codespell diff --git a/panther_bringup/README.md b/panther_bringup/README.md index ffd505eea..611304226 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -26,7 +26,7 @@ The package contains the default configuration and launch files necessary to sta - `simulation_engine` [*string*, default: **ignition-gazebo**]: simulation engine to use when running Gazebo. - `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter. - `use_sim` [*bool*, default: **false**]: whether simulation is used. -- `user_led_animaitons_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. +- `user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. - `wheel_config_path` [*string*, default: **$(find panther_description)/config/.yaml**]: path to YAML file with wheel specification. Arguments become required if `wheel_type` is set to **custom**. - `wheel_type` [*string*, default: **WH01**]: type of wheel, possible are: **WH01** - offroad, **WH02** - mecanum, **WH04** - small pneumatic, and **custom** - custom wheel types (requires setting `wheel_config_path` argument accordingly). diff --git a/panther_bringup/launch/bringup.launch.py b/panther_bringup/launch/bringup.launch.py index 9fea086e4..108a462a0 100644 --- a/panther_bringup/launch/bringup.launch.py +++ b/panther_bringup/launch/bringup.launch.py @@ -154,9 +154,9 @@ def generate_launch_description(): description="Path to a YAML file with a description of led configuration", ) - user_led_animaitons_file = LaunchConfiguration("user_led_animaitons_file") - declare_user_led_animaitons_file_arg = DeclareLaunchArgument( - "user_led_animaitons_file", + user_led_animations_file = LaunchConfiguration("user_led_animations_file") + declare_user_led_animations_file_arg = DeclareLaunchArgument( + "user_led_animations_file", default_value="", description="Path to a YAML file with a description of the user defined animations", ) @@ -248,7 +248,7 @@ def generate_launch_description(): condition=UnlessCondition(use_sim), launch_arguments={ "led_config_file": led_config_file, - "user_led_animaitons_file": user_led_animaitons_file, + "user_led_animations_file": user_led_animations_file, }.items(), ) @@ -307,7 +307,7 @@ def generate_launch_description(): declare_controller_config_path_arg, declare_battery_config_path_arg, declare_led_config_file_arg, - declare_user_led_animaitons_file_arg, + declare_user_led_animations_file_arg, declare_simulation_engine_arg, declare_publish_robot_state_arg, declare_use_ekf_arg, diff --git a/panther_lights/LIGHTS_API.md b/panther_lights/LIGHTS_API.md index 38c5cb1d6..a9089f35a 100644 --- a/panther_lights/LIGHTS_API.md +++ b/panther_lights/LIGHTS_API.md @@ -37,7 +37,7 @@ Getters: It is possible to define your own animation type with expected, new behavior. All animation definitions inherit from the basic `Animation` class. Animations are loaded using `pluginlib` and can be defined from any point in your project. All you need is to import `Animation` class from `panther_lights` package and export it as a pluginlib plugin. For more information about creating and managing pluginlib see: [Creating and using plugins (C++)](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Pluginlib.html). -Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animaiton_description` and processed inside `Initialize` method. See the example below or other animation definitions. +Frames are processed in ticks with a frequency of `controller_frequency`. It is required to overwrite the `UpdateFrame` method which must return a list representing the frame for a given tick. The advised way is to use the `GetAnimationIteration` (current animation tick) as a time base for animation frames. The length of the frame has to match `num_led`. Each element of the frame represents a color for a single LED in the Bumper Lights. Colors are described as a list of integers with respective **R**, **G**, and **B** color values and **A** alpha channel. Additional parameters (e.g. image path) can be passed within `animation_description` and processed inside `Initialize` method. See the example below or other animation definitions. Create a New Animation Type: diff --git a/panther_lights/README.md b/panther_lights/README.md index e5f4cfb18..644d0d4f7 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -99,7 +99,7 @@ This node is responsible for processing animations and publishing frames to be d - `~controller_frequency` [*float*, default: **50.0**]: frequency [Hz] at which the lights controller node will process animations. - `~led_config_file` [*string*, default: **$(find panther_lights)/panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. -- `~user_led_animaitons_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. +- `~user_led_animations_file` [*string*, default: **None**]: path to a YAML file with a description of the user defined animations. [//]: # (ROS_API_NODE_PARAMETERS_END) [//]: # (ROS_API_NODE_END) @@ -140,7 +140,7 @@ The `led_animations` section contains list with definitions for various animatio - `animations` [*list*, default: **None**]: definition of animation for each Bumper Lights. Supported keys are: - `type` [*string*, default **None**]: Specifies the type of animation. Default animation types are: `panther_lights::ImageAnimation`, `panther_lights::ChargingAnimation`. - `segments` [*string*, default **None**]: Indicates which segment mapping this particular animation applies to (e.g., all, front, rear). - - `animaiton` [*yaml*, default: **None**]: An animation to be displayed on segments. The keys for the configuration of different animation types are explained in detail under the [**Animation Types**](#animation-types) section. + - `animation` [*yaml*, default: **None**]: An animation to be displayed on segments. The keys for the configuration of different animation types are explained in detail under the [**Animation Types**](#animation-types) section. - `id` [*int*, default: **None**]: unique ID of an animation. - `name` [*string*, default: **ANIMATION_**]: name of an animation. If not provided will default to **ANIMATION_**, where `` is equal to `id` parameter of the given animation. - `priority` [*int*, default: **3**]: priority at which animation will be placed in the queue. The list below shows the behavior when an animation with a given ID arrives: diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index e4dfe66bc..4c4d7d437 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -124,7 +124,7 @@ class ControllerNode : public rclcpp::Node * * @param animation_id ID of the animations * @param repeating Whether animations should repeat - * @param param Optional animaiton parameter + * @param param Optional animation parameter * * @exception std::runtime_error if no animation with given ID exists */ diff --git a/panther_lights/include/panther_lights/led_animations_queue.hpp b/panther_lights/include/panther_lights/led_animations_queue.hpp index 73dd97bd4..8b9d8e36f 100644 --- a/panther_lights/include/panther_lights/led_animations_queue.hpp +++ b/panther_lights/include/panther_lights/led_animations_queue.hpp @@ -63,7 +63,7 @@ class LEDAnimation { public: /** - * @brief Initializes LED animaiton + * @brief Initializes LED animation * * @param led_animation_description YAML description of the LED animation * @param segments This parameter is used to create map of segments used by this LED animation @@ -84,7 +84,7 @@ class LEDAnimation bool IsFinished(); /** - * @brief Reset all animaitons on all LED segments + * @brief Reset all animations on all LED segments * * @param time This time is used to set new animation initialization time */ @@ -132,7 +132,7 @@ class LEDAnimationsQueue { public: /** - * @brief Initializes LED animaitons queue + * @brief Initializes LED animations queue * * @param max_queue_size Max size of the queue */ diff --git a/panther_lights/include/panther_lights/led_segment.hpp b/panther_lights/include/panther_lights/led_segment.hpp index 05e9bdb7d..2bda77dac 100644 --- a/panther_lights/include/panther_lights/led_segment.hpp +++ b/panther_lights/include/panther_lights/led_segment.hpp @@ -38,7 +38,7 @@ class LEDSegment * * @param segment_description YAML description of the segment. Must contain given keys: * - led_range (string) - two numbers with hyphen in between, eg.: '0-45', - * - channel (int) - id of phisical LED channel to which segment is assigned. + * - channel (int) - id of physical LED channel to which segment is assigned. * @param controller_frequency frequency at which animation will be updated. * * @exception std::runtime_error or std::invalid_argument if missing required description key or diff --git a/panther_lights/launch/lights.launch.py b/panther_lights/launch/lights.launch.py index 9e72f0516..d32dd2219 100644 --- a/panther_lights/launch/lights.launch.py +++ b/panther_lights/launch/lights.launch.py @@ -28,9 +28,9 @@ def generate_launch_description(): description="Path to a YAML file with a description of led configuration", ) - user_led_animaitons_file = LaunchConfiguration("user_led_animaitons_file") - declare_user_led_animaitons_file_arg = DeclareLaunchArgument( - "user_led_animaitons_file", + user_led_animations_file = LaunchConfiguration("user_led_animations_file") + declare_user_led_animations_file_arg = DeclareLaunchArgument( + "user_led_animations_file", default_value="", description="Path to a YAML file with a description of the user defined animations", ) @@ -48,7 +48,7 @@ def generate_launch_description(): name="lights_controller_node", parameters=[ {"led_config_file": led_config_file}, - {"user_led_animaitons_file": user_led_animaitons_file}, + {"user_led_animations_file": user_led_animations_file}, ], on_exit=Shutdown(), ) @@ -61,7 +61,7 @@ def generate_launch_description(): actions = [ declare_led_config_file_arg, - declare_user_led_animaitons_file_arg, + declare_user_led_animations_file_arg, lights_driver_node, lights_controller_node, dummy_scheduler_node, diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index 9e37f74dd..b9777f0f7 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -47,11 +47,11 @@ ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::Node using std::placeholders::_2; this->declare_parameter("led_config_file"); - this->declare_parameter("user_led_animaitons_file", ""); + this->declare_parameter("user_led_animations_file", ""); this->declare_parameter("controller_freq", 50.0); const auto led_config_file = this->get_parameter("led_config_file").as_string(); - const auto user_led_animations_file = this->get_parameter("user_led_animaitons_file").as_string(); + const auto user_led_animations_file = this->get_parameter("user_led_animations_file").as_string(); const float controller_freq = this->get_parameter("controller_freq").as_double(); YAML::Node led_config_desc = YAML::LoadFile(led_config_file); @@ -156,9 +156,9 @@ void ControllerNode::LoadUserAnimations(const std::string & user_led_animations_ RCLCPP_INFO(this->get_logger(), "Loading users LED animations"); try { - YAML::Node user_led_animaitons = YAML::LoadFile(user_led_animations_file); + YAML::Node user_led_animations = YAML::LoadFile(user_led_animations_file); auto user_animations = panther_utils::GetYAMLKeyValue>( - user_led_animaitons, "user_animations"); + user_led_animations, "user_animations"); for (auto & animation_description : user_animations) { try { diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 988504c48..25f28c8a1 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -108,31 +108,31 @@ void DriverNode::FrameCB( const ImageMsg::ConstSharedPtr & msg, const apa102::APA102 & panel, const rclcpp::Time & last_time, const std::string & panel_name) { - std::string meessage; + std::string message; if ( (this->get_clock()->now() - rclcpp::Time(msg->header.stamp)) > rclcpp::Duration::from_seconds(frame_timeout_)) { - meessage = "Timeout exceeded, ignoring frame"; + message = "Timeout exceeded, ignoring frame"; } else if (rclcpp::Time(msg->header.stamp) < last_time) { - meessage = "Dropping message from past"; + message = "Dropping message from past"; } else if (msg->encoding != sensor_msgs::image_encodings::RGBA8) { - meessage = "Incorrect image encoding ('" + msg->encoding + "')"; + message = "Incorrect image encoding ('" + msg->encoding + "')"; } else if (msg->height != 1) { - meessage = "Incorrect image height " + std::to_string(msg->height); + message = "Incorrect image height " + std::to_string(msg->height); } else if (msg->width != (std::uint32_t)num_led_) { - meessage = "Incorrect image width " + std::to_string(msg->width); + message = "Incorrect image width " + std::to_string(msg->width); } - if (!meessage.empty()) { + if (!message.empty()) { if (panel_name == "front") { RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "%s on front panel!", meessage.c_str()); + this->get_logger(), *this->get_clock(), 5000, "%s on front panel!", message.c_str()); } else if (panel_name == "rear") { RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 5000, "%s on rear panel!", meessage.c_str()); + this->get_logger(), *this->get_clock(), 5000, "%s on rear panel!", message.c_str()); } - auto warn_msg = meessage + " on " + panel_name + " panel!"; + auto warn_msg = message + " on " + panel_name + " panel!"; diagnostic_updater_.broadcast(diagnostic_msgs::msg::DiagnosticStatus::WARN, warn_msg); return; diff --git a/panther_lights/test/test_animation.cpp b/panther_lights/test/test_animation.cpp index 9f778a05c..9f69a281b 100644 --- a/panther_lights/test/test_animation.cpp +++ b/panther_lights/test/test_animation.cpp @@ -165,7 +165,7 @@ TEST_F(TestAnimation, Update) animation_->Reset(); - // reach end of first loop of animaiton + // reach end of first loop of animation for (std::size_t i = 0; i < 20; i++) { ASSERT_NO_THROW(animation_->Update()); } @@ -174,7 +174,7 @@ TEST_F(TestAnimation, Update) expected_progress = 20.0 / (20 * 2); EXPECT_FLOAT_EQ(expected_progress, animation_->GetProgress()); - // reach animaiton end + // reach animation end for (std::size_t i = 0; i < 20; i++) { ASSERT_NO_THROW(animation_->Update()); } @@ -182,7 +182,7 @@ TEST_F(TestAnimation, Update) EXPECT_TRUE(animation_->IsFinished()); EXPECT_FLOAT_EQ(1.0, animation_->GetProgress()); - // after reaching animaiton end Update() method when invoked should return frame filled with 0 + // after reaching animation end Update() method when invoked should return frame filled with 0 animation_->Update(); auto frame = animation_->GetFrame(); EXPECT_EQ(num_led * 4, frame.size()); diff --git a/panther_lights/test/test_controller_node.cpp b/panther_lights/test/test_controller_node.cpp index e253c701e..674ed3b19 100644 --- a/panther_lights/test/test_controller_node.cpp +++ b/panther_lights/test/test_controller_node.cpp @@ -74,7 +74,7 @@ class TestControllerNode : public testing::Test protected: void CreateLEDConfig(const std::filesystem::path file_path); void CallSetLEDAnimationSrv( - const std::size_t animaiton_id, const bool repeating, const std::string & param = ""); + const std::size_t animation_id, const bool repeating, const std::string & param = ""); static constexpr std::size_t kTestNumberOfLeds = 10; static constexpr std::size_t kTestChannel = 1; @@ -251,7 +251,7 @@ TEST_F(TestControllerNode, LoadAnimationThrowRepeatingID) "Animation with given ID already exists")); } -TEST_F(TestControllerNode, AddAnimationToQueueThrowBadAnimaitonID) +TEST_F(TestControllerNode, AddAnimationToQueueThrowBadanimationID) { EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:")); @@ -265,7 +265,7 @@ TEST_F(TestControllerNode, AddAnimationToQueue) EXPECT_FALSE(queue->Empty()); } -TEST_F(TestControllerNode, CallSelLEDAnimaitonService) +TEST_F(TestControllerNode, CallSelLEDAnimationService) { this->CallSetLEDAnimationSrv(0, false); @@ -288,7 +288,7 @@ TEST_F(TestControllerNode, CallSelLEDAnimaitonService) EXPECT_FALSE(queue->Empty()); } -TEST_F(TestControllerNode, CallSelLEDAnimaitonServicePriorityInterrupt) +TEST_F(TestControllerNode, CallSelLEDAnimationServicePriorityInterrupt) { this->CallSetLEDAnimationSrv(0, false); From 78e851337a42e8b885d6ab4f6b1d8ea5e2cffe06 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Thu, 7 Mar 2024 11:38:09 +0100 Subject: [PATCH 27/32] Update panther_bringup/README.md Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_bringup/README.md b/panther_bringup/README.md index 611304226..c5510f8ee 100644 --- a/panther_bringup/README.md +++ b/panther_bringup/README.md @@ -21,7 +21,7 @@ The package contains the default configuration and launch files necessary to sta - `battery_config_path` [*string*, default: **None**]: path to the Ignition LinearBatteryPlugin configuration file. This configuration is intended for use in simulations only. - `controller_config_path` [*string*, default: **panther_controller/config/_controller.yaml**]: path to controller configuration file. A path to custom configuration can be specified here. - `ekf_config_path` [*string*, default: **panther_bringup/config/ekf.yaml**]: path to the EKF configuration file. -- `led_config_file` [*string*, default: **panther_lighst/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. +- `led_config_file` [*string*, default: **panther_lights/config/led_config.yaml**]: path to a YAML file with a description of led configuration. This file includes definition of robot panels, virtual segments and default animations. - `publish_robot_state` [*bool*, default: **true**]: whether to publish the default Panther robot description. - `simulation_engine` [*string*, default: **ignition-gazebo**]: simulation engine to use when running Gazebo. - `use_ekf` [*bool*, default: **true**]: enable or disable Extended Kalman Filter. From 376cffcea140a83ad4e0b3604db9f1d7abb247b5 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Thu, 7 Mar 2024 11:38:31 +0100 Subject: [PATCH 28/32] Update panther_hardware_interfaces/README.md Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther_hardware_interfaces/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_hardware_interfaces/README.md b/panther_hardware_interfaces/README.md index d87f5d8f7..5307180e2 100644 --- a/panther_hardware_interfaces/README.md +++ b/panther_hardware_interfaces/README.md @@ -33,7 +33,7 @@ That said apart from the usual interface provided by the ros2_control, this plug [//]: # (ROS_API_NODE_PUBLISHERS_START) - `/diagnostics` [*diagnostic_msgs/DiagnosticArray*]: Panther system diagnostic messages. -- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers' state and error flags. +- `/panther_system_node/driver/motor_controllers_state` [*panther_msgs/DriverState*]: current motor controllers state and error flags. [//]: # (ROS_API_NODE_PUBLISHERS_END) From 5701d51c252a0bef4bf63ad4ccdb152cb7e263f5 Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Thu, 7 Mar 2024 11:46:15 +0100 Subject: [PATCH 29/32] Update panther_lights/README.md Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther_lights/README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/panther_lights/README.md b/panther_lights/README.md index 644d0d4f7..e77e69631 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -105,7 +105,6 @@ This node is responsible for processing animations and publishing frames to be d [//]: # (ROS_API_NODE_END) [//]: # (ROS_API_PACKAGE_END) - ## LED configuration Basic led configuration is loaded from [`led_config.yaml`](config/led_config.yaml) file. It includes definition of robot panels, virtual segments and default animations. From 33cafe6878d580b63285fe42712cf48ad6a03c2b Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Thu, 7 Mar 2024 11:49:33 +0100 Subject: [PATCH 30/32] Update panther_lights/test/test_controller_node.cpp Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- panther_lights/test/test_controller_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panther_lights/test/test_controller_node.cpp b/panther_lights/test/test_controller_node.cpp index 674ed3b19..b56ad08dd 100644 --- a/panther_lights/test/test_controller_node.cpp +++ b/panther_lights/test/test_controller_node.cpp @@ -251,7 +251,7 @@ TEST_F(TestControllerNode, LoadAnimationThrowRepeatingID) "Animation with given ID already exists")); } -TEST_F(TestControllerNode, AddAnimationToQueueThrowBadanimationID) +TEST_F(TestControllerNode, AddAnimationToQueueThrowBadAnimationID) { EXPECT_TRUE(panther_utils::test_utils::IsMessageThrown( [&]() { controller_node_->AddAnimationToQueue(99, false); }, "No animation with ID:")); From 28a1dfa20b9719d11316db412cfd13ee85f0dbd6 Mon Sep 17 00:00:00 2001 From: Dawid Date: Thu, 7 Mar 2024 14:02:56 +0000 Subject: [PATCH 31/32] review fixes --- .../panther_lights/controller_node.hpp | 4 ++-- panther_lights/src/controller_node.cpp | 23 +++++++++---------- panther_lights/src/driver_node.cpp | 2 ++ .../test/test_segment_converter.cpp | 2 +- 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/panther_lights/include/panther_lights/controller_node.hpp b/panther_lights/include/panther_lights/controller_node.hpp index 4c4d7d437..25a51dd36 100644 --- a/panther_lights/include/panther_lights/controller_node.hpp +++ b/panther_lights/include/panther_lights/controller_node.hpp @@ -35,6 +35,7 @@ namespace panther_lights { +using ImageMsg = sensor_msgs::msg::Image; using SetLEDAnimationSrv = panther_msgs::srv::SetLEDAnimation; class ControllerNode : public rclcpp::Node @@ -151,8 +152,7 @@ class ControllerNode : public rclcpp::Node void ControllerTimerCB(); std::unordered_map> led_panels_; - std::unordered_map::SharedPtr> - panel_publishers_; + std::unordered_map::SharedPtr> panel_publishers_; std::unordered_map> segments_; std::unordered_map> segments_map_; std::unordered_map animations_descriptions_; diff --git a/panther_lights/src/controller_node.cpp b/panther_lights/src/controller_node.cpp index b9777f0f7..ff3035409 100644 --- a/panther_lights/src/controller_node.cpp +++ b/panther_lights/src/controller_node.cpp @@ -43,8 +43,7 @@ namespace panther_lights ControllerNode::ControllerNode(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { - using std::placeholders::_1; - using std::placeholders::_2; + using namespace std::placeholders; this->declare_parameter("led_config_file"); this->declare_parameter("user_led_animations_file", ""); @@ -95,7 +94,7 @@ void ControllerNode::InitializeLEDPanels(const YAML::Node & panels_description) } const auto pub_result = panel_publishers_.emplace( - channel, this->create_publisher( + channel, this->create_publisher( "lights/driver/channel_" + std::to_string(channel) + "_frame", 10)); if (!pub_result.second) { throw std::runtime_error( @@ -249,16 +248,16 @@ void ControllerNode::PublishPanelFrame(const std::size_t channel) auto panel = led_panels_.at(channel); const auto number_of_leds = panel->GetNumberOfLeds(); - sensor_msgs::msg::Image image; - image.header.frame_id = "lights_channel_" + std::to_string(channel); - image.header.stamp = this->get_clock()->now(); - image.encoding = "rgba8"; - image.height = 1; - image.width = number_of_leds; - image.step = number_of_leds * 4; - image.data = panel->GetFrame(); + ImageMsg::UniquePtr image(new ImageMsg); + image->header.frame_id = "lights_channel_" + std::to_string(channel); + image->header.stamp = this->get_clock()->now(); + image->encoding = "rgba8"; + image->height = 1; + image->width = number_of_leds; + image->step = number_of_leds * 4; + image->data = panel->GetFrame(); - panel_publishers_.at(channel)->publish(image); + panel_publishers_.at(channel)->publish(std::move(image)); } void ControllerNode::ControllerTimerCB() diff --git a/panther_lights/src/driver_node.cpp b/panther_lights/src/driver_node.cpp index 25f28c8a1..b12f1855e 100644 --- a/panther_lights/src/driver_node.cpp +++ b/panther_lights/src/driver_node.cpp @@ -102,6 +102,8 @@ void DriverNode::OnShutdown() // Give back control over LEDs SetPowerPin(false); + + gpio_driver_.reset(); } void DriverNode::FrameCB( diff --git a/panther_lights/test/test_segment_converter.cpp b/panther_lights/test/test_segment_converter.cpp index 9540a7635..516d7eab2 100644 --- a/panther_lights/test/test_segment_converter.cpp +++ b/panther_lights/test/test_segment_converter.cpp @@ -84,7 +84,7 @@ TEST_F(TestSegmentConverter, ConvertInvalidLedRange) { segments_.emplace( "name", std::make_shared( - CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 10, 1), 50.0)); + CreateSegmentDescription(panel_1_num_led_, panel_1_num_led_ + 1, 1), 50.0)); const auto anim_desc = CreateImageAnimationDescription(); ASSERT_NO_THROW( segments_.at("name")->SetAnimation("panther_lights::ImageAnimation", anim_desc, false)); From 1262781226f8da782243f761a92a90038fd96b7e Mon Sep 17 00:00:00 2001 From: Dawid Kmak <73443304+KmakD@users.noreply.github.com> Date: Fri, 8 Mar 2024 09:46:39 +0100 Subject: [PATCH 32/32] Update README.md --- panther_lights/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/panther_lights/README.md b/panther_lights/README.md index e77e69631..27d0d7744 100644 --- a/panther_lights/README.md +++ b/panther_lights/README.md @@ -172,7 +172,7 @@ Basic animation definition. Keys are inherited from the basic **Animation** clas - `duration` [*float*, default: **None**]: duration of the animation. - `repeat` [*int*, default: **1**]: number of times the animation will be repeated. -> **Note** +> [!NOTE] > Overall display duration of an animation is a product of a single image duration and repeat count. The result of `duration` x `repeat` can't exceed 10 **[s]**. If animation fails to fulfill the requirement it will result in an error. #### ImageAnimation @@ -251,10 +251,10 @@ user_animations: repeat: 1 ``` -> **Note** +> [!NOTE] > ID numbers from 0 to 19 are reserved for system animations. -> **Note** +> [!NOTE] > Priority **1** is reserved for crucial system animations. Users can only define animations with priority **2** and **3**. Remember to modify launch command to use user animations: