From 507ae5ceb04b4b7ed7ec1728e7f9370170de7eff Mon Sep 17 00:00:00 2001 From: Ryan Date: Sun, 28 Jul 2024 16:57:56 -0600 Subject: [PATCH] Fix LoadFromBag assumptions causing C++ exceptions during serialization (#438) Fix LoadFromBag assumptions * Not all bags have only GridMap messages * Not all bags have GridMap on the right topic * Add test for trying to load a grid map from a bag that doesn't contain it on the expected topic * Add nullptr check on reader messages * Remove unused include * Don't skip reporting when tests fail (cherry picked from commit 25a1ea53297ac739d0b2fd6eff6a5c0145cee5c6) --- .github/workflows/colcon.yaml | 2 +- grid_map_ros/CMakeLists.txt | 3 + .../double_chatter/double_chatter.db3 | Bin 0 -> 40960 bytes .../resource/double_chatter/metadata.yaml | 57 ++++++++++++++++++ .../resource/test_multitopic/metadata.yaml | 35 +++++++++++ .../test_multitopic/test_multitopic_0.db3 | Bin 0 -> 32768 bytes grid_map_ros/src/GridMapRosConverter.cpp | 26 ++++++++ grid_map_ros/test/GridMapRosTest.cpp | 30 +++++++++ 8 files changed, 152 insertions(+), 1 deletion(-) create mode 100644 grid_map_ros/resource/double_chatter/double_chatter.db3 create mode 100644 grid_map_ros/resource/double_chatter/metadata.yaml create mode 100644 grid_map_ros/resource/test_multitopic/metadata.yaml create mode 100644 grid_map_ros/resource/test_multitopic/test_multitopic_0.db3 diff --git a/.github/workflows/colcon.yaml b/.github/workflows/colcon.yaml index 8c904d8f3..9b4eed5c1 100644 --- a/.github/workflows/colcon.yaml +++ b/.github/workflows/colcon.yaml @@ -44,7 +44,7 @@ jobs: run: | source /opt/ros/${{matrix.config.rosdistro}}/setup.bash source install/setup.bash - colcon test --return-code-on-test-failure --paths src/grid_map/* --event-handlers=console_cohesion+ + colcon test --paths src/grid_map/* --event-handlers=console_cohesion+ colcon test-result --all --verbose shell: bash diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt index b116a2325..2a75ce805 100644 --- a/grid_map_ros/CMakeLists.txt +++ b/grid_map_ros/CMakeLists.txt @@ -115,6 +115,9 @@ if(BUILD_TESTING) test/test_grid_map_ros.cpp test/GridMapRosTest.cpp ) + + file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/double_chatter DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) + file(COPY ${CMAKE_CURRENT_SOURCE_DIR}/resource/test_multitopic DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) endif() if(TARGET ${PROJECT_NAME}-test) diff --git a/grid_map_ros/resource/double_chatter/double_chatter.db3 b/grid_map_ros/resource/double_chatter/double_chatter.db3 new file mode 100644 index 0000000000000000000000000000000000000000..10eb9a495444e46e898a6a124b6f0e9c258d5cc0 GIT binary patch literal 40960 zcmeHQYiu0XbsoN{70Hz?JF+TEzLgs55o+J>SV1V8vY1ehkaT24cGt5rcgd-DXI3+_ zr05oEC_8Wqv9*TA+_#Esz#S4fID0ITG(~_E@Q?OSf&%@{ zoyX2DmlR3Ku4~@~mpgan-gD1<_uO;NoqOlp#V^boE@ySiZmKTJDhHH-0p+udDT?wa zzA=1*&nOOtg9LsDBF9@DKB^?HeQpw0M<;$!iT`dqK2@5$H1Q22RpWMikoOKfKG$b!24hHF@+<1-jNHPVmQXol_CcGp{+EzLd)jsvc_sad*VF84dl z_3CCGdNLA8FyLsm(IT#^E2^{7o#*t%($LtU!-of69ul`IAauO{!(BJzo!jajOtsm| zeV-e0xNWEn<0Vya|($>cgp+OR7+IE zZNQ=%j%!;zRngddApTzp{^Um@AQ6xVNCYGT5&?;TL_i`S5s(N-1SA3yfsYUZvEd=* zh;20*ct%9y|H1vAR;DKKPktl<5&?;TL_i`S5s(N-1SA3yfvphuqYHy0%D}fL$NlNO z3M<7h1!`kj8f?Fw^lHn7At8`Imin82*W7&BGVa=2!hx>I49QZAP%rHZ8j z%B!{ux4cR-&pjEdj$vxN%3GGUQeo+0s+1~}OSxh?n=0qC#e6On%|{VeaB$ zDqYpF_+D!vYC7tLLPhh z<^TlxRqdVyI<*Tq?le}|K)rID+Fp!rJRj4nX3Iva&|(E|c>pBqc-QHsr9*si5<-P; zxXP=vMxfeK-4%xf|11OgHFY^t&7?B9YARDr7tW?q)qJX&D^^npy_SuMRRvyVDp%P- z5h0YlotjQYG(dO9?gt1J2qnuGf(QY07jd$u|EFSF8(pher6mj%c01Nt1wgEaN z$j);Y&&%&Td0x+#>cw=bpysuDy;RSu#d0o{DQW3aI$h*ElTM}crF=@)H7#E#>h)Z% zT+gM;*-W{Rs+V3>2lpw-D+hmb7Rs!p+O)8c+f}dGY^_o4X{M_|7THQW@sDHD0&F*Y zknn6fy7K#YDwpMjOtDx>)iqTw71c~e&t&p7Eu*Hg#cUz1*HnzTaj~ElYUtgyQlYHo zRa`|kPvw67Qv>^yBjtm?HH1D6^=q}=K0SNhTHd?TCC@?U_r+sLdm{(WyP@OGi1q{b zQ1&XH5{@o^R>JXdfWy~T9KHw7(Ub{>BSOD-R+67{0FBc#+$Raty~; z+qH(_tY8&lg`Mmnb`sYg!8eg~Y%S@ngiLyc6I!dK?8h~$#jz;UvQM2>lqAxR9};{H zQ~h6g;B!WQKC{a0PjvYF%7OO=13q7w`oX7m$!B&?d_LU4^V%C@f7+kVymI@Y4xiWl z>YGnczV8Fi^toN*ncowi2RnGqt-gD_Kc5-p_JbWh=T^V=PQd36hjxu;W>0)R(82T3 zV@ogf=QF3=KG5Ow(PL8&Qogr==cm_qjc0C8e8xL?PX6;l|K6X^v~qi@!{_7={!H8t z@!9_5uJKIoh0l%ANx{Icpd+pS=kt&KcL3Cz7cPxa2YCSBU~P(xh0wN6U0PAdA6XD- z^yx$*LFYb+uh)r#60WV9PRp|0WKdT0XyhBi*E^>u=g~MD$0mr8JJTK`4~rfkkod#- zZ5d%8wAiju5Ud*4u~OPwEX_`ai_lK)iZRl5PC&0Jw2|_!g2DY!1}DB0AT{~s#d~8A z9bq@fZOP!7m|*ZJ55b|60fPsB`!m}zcxFokH!o8n8M`mcKE6*d`-EWjxt|^lnEk@n z%iA)0YD;E&m+9OmRF92En*F&Sy&f?7_OE5OWt6O5f4TJ58r(M|6JvtGc@N2V-wGIf z@8-#E8T@%LD3&X+mS>!~&D<3m%M~hY(O(ITEW2uM3Pmo2rW(?$dNiv|)i9N(sUuB| z3Wg^P!{j5(;~fNULgi2-bg!p7q2R2!?VVL*IHWK=7?M zemc6H4kQGx(sFjTcy;E&v(=#TU&r_MF!3lH zuJs@P-KoRG58|A(jk8u9<){}F#D{`c{3#{VMz$MN5b z|3>^*<93|KpN)S${+W0>{tNL#@z~V=OuaYtZ&Tl!`WqCHABliOKq4R!kO)WwBmxov ziGV~vA|Mfv2z($2Obm?<;Mt0pP&ynYFl+Qkkf14?Ltz5*G@l3(G->j1n83`)LqUS3 zDGqiLUwh}lAVCuY4|Eb!4;}~-^e8?aCh)v{%1?-A-;45X%ao%@CV{uF-dCg*%)ztmS2zbQ75zOmty2&>Ch7IlH>6Or5DVX9niK zuw@t`V(82;nXR>5qv0^sVQW~96s>9*ansth1j0+vo7|bCpEX%)ShZv}j~6Nt>mgOh z@syU$s<~7ltJSn}uE=Y(Iwm~TG`%dIMA)`QTjycvkj)eG=|b-8e7czNATb;up{`=* zDiHxpO#E3u!$#OO4~JgFuq>g58^Y_`wtqJSUenO^2AvjA*4u8|=F?P#e~a|%g@$5n zl*GH_*q{txcu*xM=OV?LqvF7eFmtFq)dus}3EGXD?ItatqRp*B7HI}mH^hEirrQBq zp#cC28ar|tT@mnpfq3J*=QYffs z?(&5z=Q5(6YpN~gr~SK#WJcB4x|>jIS(wCju~k}QooSdbh6o708Qr#p@Mtw$Xq%Bb zYkWmrH7q-*@Y>A6<#Sn4;jC?=*jClKh}y?Y#8g8?rfIndvE*2dRUQzUU0ArXa4r`@ zMB9s#l&CetLtrYk6gR~tb%Kbq(sp%g%?yfPm|2>cKbNQCLY-VNcta=UL69#68wq-? zBv|o7pRqNF_A)m~C?P?$bplHXj;&rmVjy_xuEVMRzU!>fKd&xUs&C?nh6BU zqcDo1Y(NJh2)O|Ve)cdE6l|CW@#_Tntw~bAx-fg&)I<{yN81$fvjxQ{++nCmwyBYN zMmB@+5hiLpcb`A8ON&oe*cJ^)5<$`!-jmgAwoOBW1UPKr%A$~$P{)x*tDs0|O}Jw$ zn+a0y!kSS0O%0m|Cbfp8y;w!B6|p#lO$jjZBX`28H6bl%=xMf!{Rwpj5wRJ%A890j zfs;aOcqx<$2t;D{Z6@NYZ7gwH=Ilmlnx%R9v?pz#=@*UNr}zJ9{694KGiBtpVG}>{ zBN6zxAaJKOJg#JqymDmYNT2w1fj8^v7BVZAZNL{0#?^OA;7-DAG-1SHL7bMO6ydc@I!Ua*U_*>&gE>~YTFE^5LTxt92HSxldEL@ z$u)`8a?x5Nzl<4mD#q5Vb^~6V@gnSkMLstyP@BW0@eC0d0+)zVOHPik9Q2AIBf$gj93L82t{=H`9b*VT+A4J8Hi79PtpICDMv(%q zG9XD-jLaJj;Qdhh&?dXi3PBA|3)GeSvQU50IF+ zj8+coBc~m*0}q&kd-(g{RSXeA=Qa_$8sK;1c??R{5HHxr(;;%<)lH-jJR9a)r$|I$ z=kr@2ECiB%wjGnzc>@sx?@HFs)DXB@ajp&bCrGaq*9Hfe+s5WZH5M`m&j-+&7&Or~^|aGEY2On}@a)weJm~@R@D%!@i~>ujkJ`yI0U#Mx9z0>e zn?C*+8$yeBYES?Vb!@AJ9p2&mh&x8(<1t_`z9xDkV8_3xB9G}FhuE&e?)zIU1lA_3 z$!p#OyFx~Z=zCapDkkggiMU|Zt8&eNZy_uJvSTC{dA=gV`@Chrbuv9-)57XOL^*m6 z4eV`rS-?;P>4(`uUPk}CW+dk4;N{RG0FM_M+L2YD;XgZDp2mI!)7Y?J8aozDBarYk z_9>VyrAhz@MYp?xz(C$<=!*mO;UZCmWt{7tH`Fg{`^A}XvvXI?p!+YFH zXXj>TXBl>oNW?@&5cMcTX}XY17+Sxq&>5zMDo6)t;whK83_a2?ng&GGx4Yg?gbaKO z+s30Rhl0J;llJ_ZEDGl}_4!Vds?hi1q z0fpE`B#V|OMn84b=(H3a)}&~f($9pY+q zhYm0M$7rV~iI%rd*Uq-m*x9~V_ZrxJj|vLb3{#$WQ%m#K|Jgoj-o|P{IC<)7n|8qQ zPWEhpLnp&Ma3aOhV(&KWa3!`~6N#>%@?N>XJNmHe9*NF&d+-vG^={I2l3m+vHgH{KL=Gh; z6seGuWu?FZ-Yl@aDB7z6ZEp$`eTAav8}u342k1>NdQqTvZND?*NR({nr}+W08N!yx z;hC9pzBwF8O^?ycIJ*U$V32YS;8XaqC@8Uc-fMnEH=5zq)|1ioen+?ts= z``z=|f#X?XKXN)EjCiNpaIAgohP)-h+4ncs*S6Q$wHqI-f5yf{*v%VEiLv?NDYmxt z{=&iAZ_J)Oe?EIKhZQOj1skDWI_KDfErBe2bep;)BC2YrY zBFFc_M8R~@8%ODy7I?JU(V#2X_WEbgSZOUh)3+S2byTvj8i#e55TqiZKxhU|m$YuQ zdDtG8Svc69o;~}{JK2ZR(o_jy7=NA^Gbm0w&IXTnCbd7ppil%3cbx}ZYLh&xj@#_p zwg_O{eLrk;1K)OB5ss_8_(Wz+PsM(9J88j)Q#*G={5?Hpno2#+ICZ|UCxQ?-9`-LK zF|O%?la<&WEJ-Covn@J2{+t?9AWoZ)zYf)scJTnPSWXxP{zO&uY@VI_cLslY(+Fq; zGy)m{jetf#BcKt`2xtT}0vZ90fJWf!g~05I>CCsB!1w6+e`@x>nYU{A(3?g;BcKt` z2xtT}0+%2~ZYPQui4 zjCtJiLt*-!71mj?QZ`mArJAvV{5*&-{nVh@vkAdQ=y;}Rh^}w8>#R^UYUQF)sTrm6 zN_n-kVia>~cFOd7UW9UU@dQ$%??mn4)Kz>EIxI1H8Ykij8mf*qnpdi`WwXtrNCZhD zO#s1^2vcBTD_q9M@>Uc;)8zb7(@b?{S}Olhb4+!1VTrXdv*HH>81!S0F-vqK7+0nJ zK)4QXIsk8Y76__@Vl2)r*YQN14j2QXb+%e8mP*y4QL4m=o=mN+l&jU1njuSe9b1H5 z?mcz3J*ebi_BGMgbyPyb6+FbmpK9W#(}(Z*j@6)fCc++?p_$#{ zYKlWsjCHno?dq0MXjpcoQZuV%qg*RCn^w7IH%qHlwZf~0Wn1N@wX$k83&mQ^C>E+^ zxJ{|JvMN>~S7{b_p(ZPtaHIkfALA{p02Xw3H>G2psl<`bd<3!UL#gNMGzEiDf3(*m zPNuyko|qnbO{rXJ8lnofsIHU?O=0m$(4 z)GXWNrGjmVqrJxTJKX@63(bSaGtao>FXYn69p6H~f<&|%ddEG{py}sEmq+c8IwZIq zh!2fU&yAe8yE5`tvy_vQ)Ug1`){haBy~1RlN{Ois`0@QJWf)%CiHD~O80*1FcH;6W z=FA>CxlV(o!|UwxhwE9~IKDdl%r_3>Def1eZgJQZPTnt!N8c}YcE?)oRcX+p41a8c z8b>*_G0~3l`TxvcGjBQg(3?g;BcKt`2xtT}0vZ90fJQ(gpb>Z_5qNlJ>Qv@@>FqDC z9h_4Gfk%w}VR$b+fQM%$HGYTP@qrjRmma;7vdJ(E_79ES$+U@~JERRq?w%@-8l}Uq zoh(BgPmbC>MWIhNWcQrLSU&L?#pur z=Z<(z2D76G#>_>w4YmvG^N{(0({eoSx&ww4*L$D~Sq#bCV`3kx{9U2ukn_2V>{`T- z4Ug7=FnJV9`ZoK}-yaOm;*{U%2*e7oKQXtV5Ox*7|d)x(Q zg*ihrqy= zvjDvkLQ^8pmU213N=y|7LJLch=orkEYo6`HKHDFYarpqHOY%-Wv2|yc$<%O6u*pNm zj8DYkY~Gh~0=T=7hkFQ?k_e%OCBVVzfON$AZO3dUWkP+w!|uct++m)GTfHe5TEMZ8 z4^7|)WHP4d8E2+&T?U__h9b12W$+V~Bc-)epmBum#9kn|1hEfYpGT$Qbz)|$1pyzx zViF;HH~l`iVxY1?4@9qE^9bX>A;^q{e6HtsQK7>89xQ7}sfLuIjPLtyuLE~|OkT(H zCi7l?&Rg#K)<=^Jrj!q$lQ=1#|L3+cb3et0-ZTOl0gZr0KqH_L&0ib8h0kvJfop-pk;xoPWoI&{W)30< z{^Ixl_}zcf5%pDY{onqEwm)B){C-kp#^wHbW&Ajk$su8C?(dnof8j%K8Uc-fMnEH= z5zq)|1T+E~0gZr0KqH_L&)sngRlr-ow=u!_hIA_QxJBDIV$EOt7} z#Jbqc=WJq1RLWR-p#+KQupzuw)Fz_F{*vW@zavZ7>7+7%Oaqa|{A91CbOM5KQob4bVrBcqgX1++k9Qmhcf7C>X@+OJsRn6q8&aOdJq=z$k)pUeYA+7#x)c%5>>*eD+9--dmuKt) zC6A&4-0Fzzh&zBJMoMZS>-Zscv0yn$x}ZD;ofb)JrirLjeh{*-EorW68@JYOZxI`^ zA2^75=TSdJ#7i)B;Wer%@DjnD6exixq={3~ZHK6gOH^M<)^>t0it8KMKkqEwUA(uz zE=yZ>$R8UBjTefDeUI{j4@oub6m~B;<0-}6OLviSZ#TApD3YGV18b3$mJq5Bgh-bm zj^KW|W-zCN7=(7HN zm@E!pOWhZe0t;yk`i>Q~l>#_3Q~(u?6h?}APy@k8R%_v9_S)ntT1q?>*{pTAIaM^*@GVeiKC!5khcZ7P5~8SD*#L`a z?ZfCThe>{91St5SJi1GmK5hnBTtTm9Cze01HjUb)j@-`YVwntVHa4)3av~k684;=IM<%$Vu zFskFY`IrtAv=r|?&VaD7FY34LF}f? serialization; + // Validate the received bag topic exists and + // is of the correct type to prevent later serialization exception. + const auto topic_metadata = reader.get_all_topics_and_types(); + bool topic_is_correct_type = false; + for (const auto & m : topic_metadata) { + if (m.name == topic && m.type == "grid_map_msgs/msg/GridMap") { + topic_is_correct_type = true; + } + } + if (!topic_is_correct_type) { + RCLCPP_ERROR( + rclcpp::get_logger( + "loadFromBag"), "Bagfile does not contain a GridMap message on the expected topic '%s'", + topic.c_str()); + return false; + } + while (reader.has_next()) { auto bag_message = reader.read_next(); + if (bag_message == nullptr) { + continue; + } + + // Only read messages on the correct topic. + // https://github.com/ANYbotics/grid_map/issues/401 + if (bag_message->topic_name != topic) { + continue; + } if (bag_message->serialized_data != NULL) { rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); diff --git a/grid_map_ros/test/GridMapRosTest.cpp b/grid_map_ros/test/GridMapRosTest.cpp index 221629236..e48fc350d 100644 --- a/grid_map_ros/test/GridMapRosTest.cpp +++ b/grid_map_ros/test/GridMapRosTest.cpp @@ -132,6 +132,36 @@ TEST(RosbagHandling, saveLoadWithTime) rcpputils::fs::remove_all(dir); } +TEST(RosbagHandling, wrongTopicType) +{ + // This test validates LoadFromBag will reject a topic of the wrong type. + // See https://github.com/ANYbotics/grid_map/issues/401. + + std::string pathToBag = "double_chatter"; + string topic = "/chatter1"; + GridMap gridMapOut; + rcpputils::fs::path dir(pathToBag); + + EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic, gridMapOut)); +} + +TEST(RosbagHandling, checkTopicTypes) +{ + // This test validates loadFromBag will reject a topic of the wrong type or + // non-existing topic and accept a correct topic. + + std::string pathToBag = "test_multitopic"; + string topic_wrong = "/chatter"; + string topic_correct = "/grid_map"; + string topic_non_existing = "/grid_map_non_existing"; + GridMap gridMapOut; + rcpputils::fs::path dir(pathToBag); + + EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_wrong, gridMapOut)); + EXPECT_TRUE(GridMapRosConverter::loadFromBag(pathToBag, topic_correct, gridMapOut)); + EXPECT_FALSE(GridMapRosConverter::loadFromBag(pathToBag, topic_non_existing, gridMapOut)); +} + TEST(OccupancyGridConversion, withMove) { grid_map::GridMap map;