From 37444486efdee83ec8e9307b6a394a9133c817df Mon Sep 17 00:00:00 2001 From: Sarthak Mittal Date: Tue, 14 Apr 2020 00:26:46 +0530 Subject: [PATCH] Add single and multiple marker board detection nodes - Adds single_board and multi_board nodes to detect aruco marker boards - Publishes the same topics and msgs as single and double nodes - Adds sample launch files to launch single and multi board setups - Adds sample yaml files for single and multi aruco marker boards --- aruco_ros/CMakeLists.txt | 12 +- aruco_ros/include/aruco_ros/aruco_ros_utils.h | 2 + aruco_ros/launch/multi_board.launch | 23 ++ aruco_ros/launch/single_board.launch | 23 ++ aruco_ros/samples/board_1.png | Bin 0 -> 38884 bytes aruco_ros/samples/board_1.yml | 8 + aruco_ros/samples/board_2.png | Bin 0 -> 39468 bytes aruco_ros/samples/board_2.yml | 8 + aruco_ros/samples/multi_board.yml | 4 + aruco_ros/src/aruco_ros_utils.cpp | 36 ++ aruco_ros/src/multi_board.cpp | 384 ++++++++++++++++++ aruco_ros/src/single_board.cpp | 355 ++++++++++++++++ 12 files changed, 854 insertions(+), 1 deletion(-) create mode 100644 aruco_ros/launch/multi_board.launch create mode 100644 aruco_ros/launch/single_board.launch create mode 100644 aruco_ros/samples/board_1.png create mode 100644 aruco_ros/samples/board_1.yml create mode 100644 aruco_ros/samples/board_2.png create mode 100644 aruco_ros/samples/board_2.yml create mode 100644 aruco_ros/samples/multi_board.yml create mode 100644 aruco_ros/src/multi_board.cpp create mode 100644 aruco_ros/src/single_board.cpp diff --git a/aruco_ros/CMakeLists.txt b/aruco_ros/CMakeLists.txt index eee2428..c7f094f 100644 --- a/aruco_ros/CMakeLists.txt +++ b/aruco_ros/CMakeLists.txt @@ -53,11 +53,21 @@ add_executable(marker_publisher src/marker_publish.cpp add_dependencies(marker_publisher ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) target_link_libraries(marker_publisher ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_executable(single_board src/single_board.cpp + src/aruco_ros_utils.cpp) +add_dependencies(single_board ${PROJECT_NAME}_gencfg) +target_link_libraries(single_board ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +add_executable(multi_board src/multi_board.cpp + src/aruco_ros_utils.cpp) +add_dependencies(multi_board ${PROJECT_NAME}_gencfg) +target_link_libraries(multi_board ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + ############# ## Install ## ############# -install(TARGETS single double marker_publisher aruco_ros_utils +install(TARGETS single double marker_publisher single_board multi_board aruco_ros_utils ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/aruco_ros/include/aruco_ros/aruco_ros_utils.h b/aruco_ros/include/aruco_ros/aruco_ros_utils.h index e622284..154b1b4 100644 --- a/aruco_ros/include/aruco_ros/aruco_ros_utils.h +++ b/aruco_ros/include/aruco_ros/aruco_ros_utils.h @@ -21,5 +21,7 @@ namespace aruco_ros //FIXME: make parameter const as soon as the used function is also const tf::Transform arucoMarker2Tf(const aruco::Marker& marker, bool rotate_marker_axis=true); + tf::Transform arucoBoard2Tf(const aruco::Board& board, bool rotate_board_axis=true); + } #endif // ARUCO_ROS_UTILS_H diff --git a/aruco_ros/launch/multi_board.launch b/aruco_ros/launch/multi_board.launch new file mode 100644 index 0000000..b8ed0e6 --- /dev/null +++ b/aruco_ros/launch/multi_board.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aruco_ros/launch/single_board.launch b/aruco_ros/launch/single_board.launch new file mode 100644 index 0000000..9462086 --- /dev/null +++ b/aruco_ros/launch/single_board.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aruco_ros/samples/board_1.png b/aruco_ros/samples/board_1.png new file mode 100644 index 0000000000000000000000000000000000000000..f399645aaab66d595730670a0b54b354a6af8ed5 GIT binary patch literal 38884 zcmeHQX;@R|w%&jOmO+LV5Rfr~3Nna-kx4|wl&MxLa6kk>qJ;n^3PeD{Ac`VGqKqM- zT1&lUMurG77ZQP>r81Z>2Vx5{BvObn+%KrT=l;0oIe$*CZTH>}y0h6+-Zgw{uWzlF zq%T|?WIj>-1cD$L)X_sT{jOro%gr4ocbwZat;4D>cW+^+5Y*Eu&@<=WI)(?%tIycq%YKO^k_lQ zf${TmJ{f(yv^=nHJ7?o8oIb7gl$JlE_?D;lmUtts?qheQP?p^=R_;oKVM7El1D=@ zMjsq+I7(#OlwD@^R}K`cgOqp_d}-gpZKx|YmnPj%L{GhwAzNzA-+l^yYya@}eM3ws zuQ%0^zgjqXTyUB4WMJ3IqkGdGc^-_ELJl*^_?}FFH@#s(v%<^*GZ!NQ%~H;KLXCHq ze2AQ-pR-9Q;*|c$5S8~AjCSEp)RA@ZyBW7=wu@a$sy7t{L9)eb%kC)?4tO0~`d-E3 zF=yjNvZTE2BpaJ+0#@jIyEKW2OxHs13?HGnBOQ!MgHaXE!lPj5s*@(yt+^KW~KKjhU2_nbZlMLjU-(BT( zXR|+Aqbr0jKrp&_b9&Wy-C=+?*`XrGu_DKMs#*}hB%29N3;LpTW4J{}L~351o`rnu zmUpADgFk2gKtdS16xN%BVSW<|JLgpTW5F3Br)>Gkubd`z*K(pnTdxNFb7Ceh`PK#> z*-Z(MVX{4tO98V`50&CnsPREMuj=!rNOd`yE_s)CqRI_A`~o`$?jZ0#>4}M>HxZ0iia%7}IweLB0B=>QRZmtXdFACD zFR;l*41_ZmBleM^{rUo^;CmA)!Iv#yF7HHPJ6eC;<<|%K)B|^BNa*8VwW1(s`fe_H z%Mr!S*gdJqMb^Z7sG;gB-{(dP2NRnt1QO_$Kauk$>A6~4u}EW-(2P9q)#a?wW7*UO z<`9D6z^`-;#-Zwo2)xPS#8DUS(V32T6tN^F+W7jwci<``IS5A51GAW;L@%Jf9z@Pw zPsv7gUP(UG`OSS9w^{mPMRX5qC~rk@o@hN_R`sTgeMv;9E)Uh5kp-fScX^)xOq!hq zE)?b^Gp8APY&yS;si~R?o$uu`PQIqN_ z?h~&&xwya?x_;FhIU^8f@|HJfmFvbEirQCO5g5)< zM3Bj2Dx!$2?_8p{qj=*<&s#nKF8r<98?3hz_EMNj-Wz|S|7m7&Q68hdED$m|{ci9f zghyDPrjMhDk$UyDBj2WSUmzU#^!^vR$Q(|zM@2f+sXZLOJ|_7LZePoD;|0l1Qn5NH z-m@qSiRDEs;mne(vc_qa7Lx+23~L~THxlm9B(cO*cz4L-Md(6Glr0PRyukq~Rd1j| zLwuN5#;#<{&Nr-CVUE3^L)xE6p_ct9bG zVu1T9m{~;ArS>;Z85DOlO?QOzgS;7${LyJs{qi!@;4YpJ6e|fmg6I_TCx+mv`%xwIy6ovu*z31m7ZI1Monqx+42irtX3+$ zp>w!D@oM@*0n|b2Oy}U1G(n6N9)D!Njmv0gi{5(cc zkHgG3EFy3V*$Ym;u@Ym|XD_TaK923l@6lq))#$IEFBm@)b7viG2>KchWB?Ly(6_D* za8xUV;V+{MBUVSM*5&F$_-J2cn0^f5%Nhhpr@`-C0EU0x1jQ`M*N4nTl|SW6sWhAT z?j^hp9)W<||1wTTXv-jq69zfzPl#`^a^4jij zq6bssL)&z*Ce)I_RC{EsmA|4e#A!VrS}Kfhgj;E&RB;IfNn4`5%}$9itfvXYs<1A= zw6Isd@(*KS!){jl*kC$5a!sI^bPH=816yUY27S5rSLsJf#-N2)0RdBDw5mGYGueC6 zPXSWd(`~jvUyiK|5qh*nHt@J(k#L6RE;2o7}iNUP-#8g*c!C*@=A77#hBJMjO|{yMOUPTNP4`q}DM@_z+jh{XTv3`_zv12aojse-J(Gz!Jk* zcK*Ujn50;gcgWUeNo9u-J(@P!0aB8jX;y7jZB1-+{zp7vBX--hEw*i`J8X{*)n1yF z6OW+TM38jJy?M5KG))mq?{AeE&V%17BhSYA$eaP|o0+7&*Oim@o|6ggOLOJ4?0<7Y zwhXsHY`X7Co_Y%uJ~bWlK!%1g#@AX-VdwfKC??`~mri7B- zX9d^#g=bj9gDTVrhJcKjE!d(U+3<*=t4;PF@cx4qMNI zJ;LxrK~hr~*hn@^2Q0Ug^J8+4)Od0)U68g9CT?1*qSVn%@1<>#+7KuQ)~D;ijWE2a zUiMs{B7h*e-1|8KIBJ1l2w(_6hT-E3-#NCH;i+nFTQ>SEMW_>RCnPHEP&lY_LoSFA zcjK#4#TO^0<36}9?pdRU3?CZRc(pDO0iR}=N!^p9;s+u0#sC=PW1g}5`BM;Synjd4=4TqXKD)e1+XswV<|i-2}1z1U@KUZg`OV#CFNO2)5UhborZBU~pc_>IyYxOcCc5j|+ zR$&e;uvMEsIT@+X*g-QD-M7P*X=X1xx7EC+*Mc7#V_9`ZxsD>yRysCtIm;1*FyZ`E z?S66BPU3;31uxo8x0<;uFaEjtYtu5a%|ucw>9dGyuE~w&*D-RlhCj^GV?calzErJU z7*X!sU1>_d%vneMsmgNdNs8FtYXLs)dMr3nfFlJsQUH~=upz(@z*_KEF@&|U@DkH7 zrHdNvF~`3v+8CdCqxzyqWtgAIOR3vIX}iUZ4UWn8Wd&6~QyIUyJ%8><#TmO}oLM(`KYZx|+ZE`P}QPaTbO*j{b12R42Cg%r8rKb!A&R&kuBZTxQPlM>oF0 zQF{DWyUE$h{Ai}MFnj&aL^`{XMw)bd@q~L8bKc{uwC-%%H2t<|@{a~sOs`4{ot5EP zr_;{)Ot7RLD6JdfX4LVl%|t2cY~^|^bq>T+AR=PrV?u8`lPxM33m~)eJ?=_SJh(?5 zD__%8NKpY6n7BuX0?bS#L;+TYbrk-1WzZ|=0(Z_&!vJ=Ye4HBlHse9i?oA+vf3V69 z{OfY1b^H30L2#p!5QjfqqfmG z9p_DST5*z#X!}aKDtIpqyXBEPG@qO@l7OU5&aWrs8%tk-LE~*u*K<@#rzN&mq(0o& z!J9M{uvTZy!hK~!D&bt%ydOPCwjIviH9*|V<&k|}LZyZXoU-tL0Sgig^LYJYJk|7-X$Y zbi%ra0we8Kk**yMvilTf=nO%%*PT~1V{mJ8&SAlLJagavl8$z>ywhm(Z~hMTnW>XB zynUGJeh4bv5U`t+&8F7!ZhEDgp@&z(?Y-{|zKx#I=XW6(*)(J=7RLnqMpQ?Qi@lG? zc?LMgut^I0c7<6sXC|ugrHGF_Xs1ceA05WzM%fs5uN(pFdWPv|@lF&<4kb zNsF(z$QXtM2{GHzUT&27UdXw^=vfhV8Uxv`SD}rPvgfBG7_KsE z5TtVB4*ocilNgI$*^Vl|GW1N5YgJWRfTr2ZoDxXiL;)sPzL83Ht6I8|cAY>MUS^liJzFXB6+dKyX)sT##Fx=UeVJ092>zj34kDyFpBUuGE5YB>7UIqCK``XSy756oPRg~QT`@61 z^%EYXzDZB~CF1GHl{RTeN{IKNT`PlCD6D5M7me!eUR62>9PVhA3sK!-j(iD4EaImG z=kfcSWreEcrfLQm0^&G={NChRObk&G>qzt~M;hEx0fNdI6gUw%t+8lpOH}#Up=WB7 zR#jQ|NQeg0bCA1`utWFj{fwW6FmDIRS+7u(j1psB%VprrGTM+dU4|d-MPr3`4uZBP za#Ec|as*P&impCk!^{`)u@kEQxAz-Zzo{XSmPGaJK>i6aA@%|2p&VB|6q7zG)*a=YNXJwVn-jRce-0!NZY`K+wGaG?Q|wU^6F}Wm&OHd6F%Wvg=~?s{-8R-}F3k*SgZhL3nHUAZcSN z<#tEKnTGD@$YOdm$%}lGs~&CVLgiQIVteAcqocjgFF~bRLZ2mXT33r%(+yz^A=>?K zS^=KPW!rr_HM=U2V-V9TyUW9Nw*Q1VT5yF@t;3zH5SM{&&dDBx_PvyO8_WvtT3u_G zTApe0TxkT%Vm_!s`Y7J1cd~|jo21$HR*8}}g*{)NrN+Nj$i$VKs!d{07TjN>EN!1u zv4K0z^TU2=9&juDy#m`gOynWQ2}-}-86d|nG9qE65Q;#1$F@t_uiPSyZBpc{r}?<>?^YX8-_6m}yuG}{El zoUDKEEvA0zGo-osme}v<%K#jtW6s>z$W(|Z1WXsdY{A<*7t!(sH*AMs*$PENdb8S)RyyB zx=B1PPz3517g3{sId2IQBK_r}n!Ab#XFt>ez?pjCu=vL(XFs$z|G$PMYftL8vi5CB zIQHq*lDwb|+9C=flAjPntISmMw<{kwRCPU#0BI*2K(u|gNn*7j(nZ2@+;`=ohY+#9 zd=DDjn+~^{!mXx%yqSaEPg@#8+opI})|b#dRb;O+Iwr|RSa7B#8%3K=30blFj3Bhr9H@fty5qqS7;|(hpo%R z=XJ`&g>J#m-6d%IMixQHb%1ChDigBg3#V?1MOFxD2blQ^-27EYky%|*4 z4$9_*fEhI5qGkUf$b@^t;NCE}Hw@$!U_*c*fVBWN1TZ?nh5!-LgbWqmUzDB4Ja&(4 zFU807krvubr9-{nb5w*dHn|8HLvQU$Ta^7kqcuOM7$|4_lPbGVJ9h3l;0sNe>(o*o zy|UtkZZ~NRKGpl9X>tu942@fP!#$j`i_$J#hg?Mb(rU=hrx} z5i|076$jWGM=WnfwmOe#G>H!Ej_G+{rw8}n!2LIH7Z&hWupz(@z*+zs0zd#Z1OS1( z=%||ovUU8;gq-+Ny|?8Cf`O%_Xz77aWnq*-*emI-<%Z3g_lue@NVBi=SOmG6f~jY@ z>b#8?tHa)LgnA>q5q+oMMW3~EV&$q(xM09kW2z@kjFgeCOY&E1uGJL8IFpJWi zcaIMD{$D)kF<~>B?N4@pGBTHK>*MQ0IO2G#Qt6S%*UdVGFLxCk{l3s}F!t)z&u(qo zb+!0_z5b>o)q6*LL)+T6W}#-R8vvt^_3UC-^yUWN6ah@-1=FiSFa$6J5DWnf0T}ZC zzP=#uz`;-mQr1EpvO59iZ~tZ5iWAk|9#IY$xgJ|+b~*d)yR6yih6|9EvdYn%v;ZG7 zald$CuZB$i91^x`?roImuv!aIhV#w)S{<(`Ki0@6)srg%rixO;o^Gx_IK_>h8u;FD-clSU8jzXMV0WgVRgKoVRrfuyMgQ|U zjHIIrkD_fjH(|9mVH?{M6MyY2!}xB@%WSW)R>|daO=?M`R@alIu_K0T_fWL(Bgb|X zZQg%?%@T&XAW^r5(t2Mk1&jY*ivQyzxrAj0t1qmXA1MPZY@D!h{%MU<815`}pqfPl zev~fDw$ereU4j@d>I#|F7W$gfbw}IKSL}ik0TWtr8qK!O)0G%^)rHjPmF|F;>#|t6Ze@$=Ky(2OI$6}(cXP*O2Gb|t`r-*RN-e3)^ro!qH?k> z9k6TdH{6sE&P^)7UR87)B}%=dJ$%^wpp=6^evhlmlJX5wu=0% z1vZ|K$4L69)n&jnI&keCT)X$j*Y5pYYi3t5T-ghXlJ`t**eE#ddUW^hBc%>ze#b9H z?)G~XPI)2={Z=EHvJoV00hS~Jnki|UvV9JET431)f?teTi$h~Q1 zIDg~{v({{S2&XYbKRiV5i9)u_taNur?Q2n3u6Xz7eAg><)@yXo*H~c-R-lm|Ti>n< zUK7q0+fh|EzRSqKc^Ehk0~gSN)Z`y*2x;SkfC31LD-^P0O{&&0eOc=788vo##}9Z= UcbKRK3r!St*yT|1L7%w)1}m)S3IG5A literal 0 HcmV?d00001 diff --git a/aruco_ros/samples/board_1.yml b/aruco_ros/samples/board_1.yml new file mode 100644 index 0000000..3b41e96 --- /dev/null +++ b/aruco_ros/samples/board_1.yml @@ -0,0 +1,8 @@ +%YAML:1.0 +aruco_bc_nmarkers: 4 +aruco_bc_mInfoType: 1 +aruco_bc_markers: + - { id: 000, corners:[ [ -0.17, -0.17, 0. ], [ -0.01, -0.17, 0. ], [ -0.01, -0.01, 0. ], [ -0.17, -0.01, 0. ] ] } + - { id: 001, corners:[ [ 0.01, -0.17, 0. ], [ 0.17, -0.17, 0. ], [ 0.17, -0.01, 0. ], [ 0.01, -0.01, 0. ] ] } + - { id: 002, corners:[ [ -0.17, 0.01, 0. ], [ -0.01, 0.01, 0. ], [ -0.01, 0.17, 0. ], [ -0.17, 0.17, 0. ] ] } + - { id: 003, corners:[ [ 0.01, 0.01, 0. ], [ 0.17, 0.01, 0. ], [ 0.17, 0.17, 0. ], [ 0.01, 0.17, 0. ] ] } diff --git a/aruco_ros/samples/board_2.png b/aruco_ros/samples/board_2.png new file mode 100644 index 0000000000000000000000000000000000000000..791f33e143bfc7d69c38da35b3ccdbbefa114637 GIT binary patch literal 39468 zcmeHw2~<;OyY3bcK}IJ8nI%A5W~gA8al$PKwzeXQ$P~~rNuppRGKd3KW!NYX2ti9L zdPRm%g%o5~5{5(^7zBbsNNhpCM8PP-`4Y7MT|MWnd(T?u@Adp=@4Z-THhX-&=Y7BT z8{TK1-{s}5w0zxi2!fR0?OVQuAZ1nYkDLtHQ&kyO0e(pZdAM(Z7S5%2)Puj|k8JlP zK#(yw%vg2F<-^}-fSt>Z!aKJv8(yNjLRSBmD-av((m1+x@6pZCk&(ElqtIqNF7PNW zL@(~}(L;Lf@XlR6$;xXXNDqRyxP5p0+4$helEJrbt*_aU77uB+SAWR}YTW5-bM6@y z=5cRE{n)fJdE-Jx#-X6ERf!L=MdNP0p-JkOr1jeoGo$_{TIaP8a?#4ec5>A+O3mk& zPza7&Z1?H337>Jsbno47zUE|>btti(J$7vVaDDyc?~(QO@e|CdNdhZ6s(JV2zrgZRUv+WIbwx6GdGYe2-08)QU0d|T zfPu5tOX)A}TD?LJToEi+Q(fg2cWsr~w7Bubdh}AC16Jx3dIv5_#h@n2M3CZ}^H`W1 z(b3fYp0Jj1fW>&Gh1|g!6f%eX>jZCQgCwX-Px!m5_D02=wZii!D z9PaU>GTpY#BD^!Tp7la)byDVQWl3oVv`nyTD)oo@(#b>ZWD9QNU=*vbFKWdSb&W+D zsb9Kp8fxVrJYGj0BXm8gS%_l#5)%q%?%7ru!RBpFMB5o|6Nc#{sJNF?R z28L;SpeU14a7*Bt-~bEKMGu`PDxGqSVdu-YXyzs~b03RAzBjEIk+P(P?GQhEQs#;8 zowJ8v#4pP@lr?bK4DRM3=PNvDw#+!e8U^`=8{pDf5YIY??sRdy&WzoNwKnDpazuHZ zXZ$jXtPANK{2P9`XULV&Cti#JdRrPLQj4-8+J`zQcdTH%7tizSsLl^G>#$dGnAy;7?lVWE0FLEl`ekhpbzdxdeM@Z}1O;4HW=0#rQDp3xCXDR9 zzRjDuQ5FgY7H9c|1AcS&c+Z3Jo3)1NJ1;q9S8>VrIu`CYzPKm0PlG%9Fut_|J7WZM zEH}JAdpoD(hZ&awjq9fboA2u-V_1hBJsvY1DI(F{G+KcxU+hdbgzt7xgvSx?%bQ(2URwEYn2s5fIFGt) z6TIJi$4Y6-FMzc#s}*Tw?Iz$RI+{K9|5l8D?>r&&tjom^<0m=}`xZRLhjFMF(I#qt z^Xx%Z(4NsOHBHG2dn1na1lD61*<8HM$Nbgius*5_8X>ZrkpDc8cx21*f^jO8tS5W>wSSI*nO z0*~EA^|=^P_mh{u`*!Gi=)#~%X;7crk8l}e!XAJwb=_Rq`QZ5J_p@7(kV>3vfF&pw zS$+qGIcLLn%*2TLZgy4e)arlrlOu&|U~yu@7oZGAXjnMg*(Wr*+Yp|}Uw1EhxR*o8 z#t{Enk80IvY5Niad6|NgRHrJ%R%3y}~3=`c(%w7Xt>Q^eeH49_Rro0`2 zkutAK9P5~fXn&@OmQ=&S(qz@7M&bcvQ2xj?Z#?y8SF>^2EBrn+nxoym?|`wa_%P>4 z2|321V#9lIUzWyqweN`I{s!JbI0GZPw1<%5Sy#D+=cctmiI0{EUSTF#R%+sV{!C>O z>9#OFa8D;iMDWexHu_mv#ctK{#D8boQ9YE%ok7OuYc(PDQ$!D_;UUQZrjd9Qx~Wm0 z{oD7Qt=Va>1g@pq17bf%XY26&OhgWBF3G;rU}ZJJ{q{?As-N-9F(J$h(t2Q<3h8DBw>z{mH0SyZLgh1L z(JrM4->M-UJAbJ^T#qE~z`Il1e_Z>3+O7>FalTu?`!?<$NY9*XrGX2DX&uZtM(-6! zTOM%^TAj3{4;jfRH8}q0$2s*OTGwD` z=<$HNuxnv-ND{0B_|>NTHwk1y60{aBs@P{4RA8e!ceGm`j~nfZC|4{xzC|bJPIlmw zPDg$GmNf1Pk+cW~|7G*Sbux=$ZW@+rwnVAcxYf9g%T|ERhfzn$d9C?fRX`i)QUNPb^u5$0VTu#oeVtC-_=XO^6{&wG?}8QK1yLh zcA$*fN|yuf0agmEkuS!lQni={kBC|qI_Xc=_bLV1rHX@+*c9>RA-j!x4dhQpcZLpK zF^IWgsGgicI6?*COS!3n@2`pm*et~%!T6h0t**NnR^zV!J`!TER~D5&n!!MpKlq2q z#p-Qb3ndNe1uPTnh*3iOsUQgL)00>-+G#0<0VM-U2DG3^U&xj#3e z>-)GvYH|}z>sr>eoNd^-FC}TIz=hcR+B#uKut`uysocEx`F+v|+3$sQ+)I4>DcPV)9bNgZ}_UszF z-*@9vE*L*r#k^#eQKo%rLn>GgDGk>cck5BNhfkGf%QSiOm^Jt_DvC{OSVF#^cC%D5 zks8)oK5XQasX4%|r!qDaWoRqNe2h~-lmymaWz)l9+B_qEWz@LB7ZWS&&c~nbG*yjT z@{TpE6O&c#UhbOKvGA*u#O1o8aSRFpghGHq07mmrUw}e@vH&#%5E-~iVwVf~e#1{| zbK|s|U(DHB@Z&*$i4WnJoC3)EwQU3oH{_`41Md{0`##{i~6 zR=nPBl-|EmPL~ftAo+tXakGI~(d`$=moMxF_ScTcks?R7)xOoBKPWONUp!pW-BaQg zKJ~D0X0bi#3TEmpZv9W`Nkb$0e`-XJ){viS$k|rT;Mm^pw=Gfkw-NNDU0c8UGDdZ~ zX+(r+wZ*=uGubA&Ejy%*R@y5p*WCH_(dAkRwNehAEZ<4tK%?&v=fn7hXuK>W_bFLI z@6SXE)lW$ln?7Ytpa-EiK_~<$1fVAWEPVl;HWKU?CDJ2W@bl@RipG125=OKxK#S$` zG;oxeC^J!Jex|iAYMlRt#wiYEW=AkN$DQnWP3m48T_IByN_rZh$CB#TyW7~AkWYx? zN=1iKin6$=oMXmgP4gSuI&>-l@c0Y_Ls@RGl=vHOZJcTOOiG3YRmXmYlGwX`()+)3E>Jsr;M(q+7N-agt@d4he|k`1x9(4+`6qXm|77t?IM!KP4l^+UBx1*R}_*+RdeC1!`R9*z_U3?Ut!=+*VqzwdXx0BF6!*H^~^mXXA2D z^b2++v|B5lDl7Sv43@f>7xqlatCCpNX-04Y$?AU$=+wA@GG+~4b-(y=nh&a7BrZcV zYg>#0nzb!P;qNCyCV4|QxbM}BK|N3i{HPVuhyFG)>T0dC6T-1pWXEnsz3E z;h*Ah|4g9yKf6Td=RBN0?^RexPwNY;Rq?0_!9$1LKl%GHK5cy=d)SmQOmbEUjjB(a zn0J8!PIT^-IN5-_NcQ!Jw)Z8T_#LI}W6OvKTeAJKpVlPK31=M>3qJ^sq&eLnc;6j| zJgwpuh2T_}izFqj|$l#3}7t_`|VUPYMZVevu9MQC>1Zx#4nNyB|-m$G?*; z*j_q#a%Si^KjhUi#~F?z8AG>a=9-eoDt`{s$vQT7H(Y|1Y6ORs; z0PUrmu+WeQBcMvQr*6#G1?XL=g`5ZKPyg%VOC^~yab{!yJqKCdUL5=l*cF7+g7n)f? zyS?-zeFd^ObJ%+FWWrN3Q9nk!C(W2DZvOvvvu1+Ev)TCG*X?YniROG9h2ndn}xx~4^ti^n6cJv zJ?UEg{fXQm#FVkJU7-Q`c(`6cgN-%Ew~#M6#sD)$xJg# zE6c-}_1hOPZM}p0gV@mfjEeDN+z+s0!0M!HA8(o5n`c6ZM!0#TL&F_x?mLVop3mql z=*!?XlLy15hj6=*ts;t{Y=CMgP`Y^>nb-o49GY6_!VdRQu_GV zWxv8PW$TquOq}n%9!8k(NZMc+$AkV=`b5~O<KB z2)q0)=L=Ls?39F`Jn^JG{A9n&S@^wPZG6GKlD3!+j8AK8mCgffv)rO_aJjl zyFzEJ3ZuDaNyf0??d)ia&p>8YC6=j8$FX}4a=S6cq0J07<=wU@*xF^NQ@2JDsFX>9 zzh5V02i?SSWgP<21{m+1fQ{k(5#dGK=;8h#J2VE#p~bQpWq2PT-ba+ELa9SQAINSQrYB00h$cF9+)9`FYk!UMYBs{h#Sf!XG7Nj!!hb>QwGT-piwLEAa z{F~bGKjQ!=AV}6#PbT2%Urjk1<82I^dkVIP68zM7Pi7A%96B$ST1vW!0V;F5ij84g zZ;J2-B&4hNC2Y`&aPMQ7WYLbg+&e_W{X)_NyO@BB-5V;tLBJm!zz3iGy%)pGK2P9m zM^5T!p8&@5)~P3#?)tFL~nXa2wAnOR!Ov{uAdX59Qp6O0>U+Imvnb3f_X^| zdME!whkz0JA>5l4gg7U^qZJ_W6fD;-g2SIjbY7v`!T6odU&rvm@FYZuP6!!iMQj(m z=$O!;>lqvS=G``^^k-j!t*sTrrv=yp2c_8J=ii<2wjq*ZR#tIG8s^ zbMCKn?iuaIFn5ysY{C<=4Mja9YZct?*wEcNZN-8~m^b=^f{OT3o@5Q6f%j{E)lip$ zFhwi2fl$UWAb!Ohq{oN6sr9?lQ{vKIg zD?E4*D%`6)7#@PHJN_;9ZO-x`BDMk9M-RAW!L zD|5Afw^zQOXId!-`n4x+;(+4%;x_^$f@lBe7-~!2_-LhKLm<|)->ezZ zLaJ=SG-UCHMh!pb;3G@_6<@kZRgg+)7CO>jk#zxa$!! z2&!>wQ0C4JhoQ>+XeIk`cI^Muh^t|tBD(d#PT6nNw-%Zjr)`(d)lX8s3fU8+SN|fF zDt9v}RX<68h4oI=mx(hJ<=R`?WBK8rQv-s~v23C_=cy|%E;&?_w9vp(K(WE;x?+`*EKp}uo2v7(>t$_Lh6oUVq z1;l~Z!G4!nhIQo1IDj?)x`%%)YiYx15@#q4VrN*e-{7ILCGy`iA+rjzicf%bTutFIY220touM%B#{(( z9s@pE7fmvH!KzDI)G+Jq0CFQqlco}RGZ_UgW7gYbF@{0)A0QM06aol^0EGZ}MAR3c z5d0l1m`X4B&{{!D8nz_mYFM@eT;8#Hq){D1C zY?`XRZq=&bO$N)Am)Pku@9|GkwL3%;mP6@l#Z#0%@2C^o{~9;TgUw^&IC(jg7cb|q zJ^VBq)2SDZ$4)Qhkh4Tg4Yg`3Z+=+V#uMJWOn&w0rNh4BkmgfBqsFiT%4UAGf{EAjJC~$mCc?Pu>2?YQ#4dK&%W-qUrCrXC6RU)DHhc^sd6SUzY?Sc=|FQD?0sKO?MLV!X5CKjT;0EGZ$0cr>!Fwo;hwC21{Hwbq*sDif8lV(49q^#0n9pcD{!%6&*>NrCdQkyU$2*fs z?WoS%bgrU;BmR^j)xL4LF8g~ZOYjs;KIE5{%WoD4*OmRB}ZPcO=pe#TQ;WP0Ct=}gwIxGE}PO^kN#hMw1 zL_*A2Lk@#Yj5|9R&uZ4BGc<{L7Gb^j7DQx~u)U`e_x$BMgfrtu`b(+CK`+~4T! SIENSSfVX;WDcuxs>fZoW6ik8u literal 0 HcmV?d00001 diff --git a/aruco_ros/samples/board_2.yml b/aruco_ros/samples/board_2.yml new file mode 100644 index 0000000..80971fc --- /dev/null +++ b/aruco_ros/samples/board_2.yml @@ -0,0 +1,8 @@ +%YAML:1.0 +aruco_bc_nmarkers: 4 +aruco_bc_mInfoType: 1 +aruco_bc_markers: + - { id: 004, corners:[ [ -0.17, -0.17, 0. ], [ -0.01, -0.17, 0. ], [ -0.01, -0.01, 0. ], [ -0.17, -0.01, 0. ] ] } + - { id: 005, corners:[ [ 0.01, -0.17, 0. ], [ 0.17, -0.17, 0. ], [ 0.17, -0.01, 0. ], [ 0.01, -0.01, 0. ] ] } + - { id: 006, corners:[ [ -0.17, 0.01, 0. ], [ -0.01, 0.01, 0. ], [ -0.01, 0.17, 0. ], [ -0.17, 0.17, 0. ] ] } + - { id: 007, corners:[ [ 0.01, 0.01, 0. ], [ 0.17, 0.01, 0. ], [ 0.17, 0.17, 0. ], [ 0.01, 0.17, 0. ] ] } diff --git a/aruco_ros/samples/multi_board.yml b/aruco_ros/samples/multi_board.yml new file mode 100644 index 0000000..6c9391a --- /dev/null +++ b/aruco_ros/samples/multi_board.yml @@ -0,0 +1,4 @@ +%YAML:1.0 +aruco_boards: + - { frame_id: "/board1", filename: "board_1.yml"} + - { frame_id: "/board2", filename: "board_2.yml"} diff --git a/aruco_ros/src/aruco_ros_utils.cpp b/aruco_ros/src/aruco_ros_utils.cpp index 2fc00b8..5f3446e 100644 --- a/aruco_ros/src/aruco_ros_utils.cpp +++ b/aruco_ros/src/aruco_ros_utils.cpp @@ -78,3 +78,39 @@ tf::Transform aruco_ros::arucoMarker2Tf(const aruco::Marker &marker, bool rotate return tf::Transform(tf_rot, tf_orig); } + +tf::Transform aruco_ros::arucoBoard2Tf(const aruco::Board &board, bool rotate_board_axis) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat Rvec64; + board.Rvec.convertTo(Rvec64, CV_64FC1); + cv::Rodrigues(Rvec64, rot); + cv::Mat tran64; + board.Tvec.convertTo(tran64, CV_64FC1); + + // Rotate axis direction as to fit ROS (?) + if (rotate_board_axis) + { + cv::Mat rotate_to_ros(3, 3, CV_64FC1); + // -1 0 0 + // 0 0 1 + // 0 1 0 + rotate_to_ros.at(0, 0) = -1.0; + rotate_to_ros.at(0, 1) = 0.0; + rotate_to_ros.at(0, 2) = 0.0; + rotate_to_ros.at(1, 0) = 0.0; + rotate_to_ros.at(1, 1) = 0.0; + rotate_to_ros.at(1, 2) = 1.0; + rotate_to_ros.at(2, 0) = 0.0; + rotate_to_ros.at(2, 1) = 1.0; + rotate_to_ros.at(2, 2) = 0.0; + rot = rot * rotate_to_ros.t(); + } + tf::Matrix3x3 tf_rot(rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), + rot.at(1, 0), rot.at(1, 1), rot.at(1, 2), + rot.at(2, 0), rot.at(2, 1), rot.at(2, 2)); + + tf::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf::Transform(tf_rot, tf_orig); +} \ No newline at end of file diff --git a/aruco_ros/src/multi_board.cpp b/aruco_ros/src/multi_board.cpp new file mode 100644 index 0000000..2c84baa --- /dev/null +++ b/aruco_ros/src/multi_board.cpp @@ -0,0 +1,384 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +/** +* @file multi_board.cpp +* @author Sarthak Mittal +* @date April 2020 +* @version 0.2.4 +* @brief ROS version of the example named "simple" in the Aruco software package. +*/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace aruco; + +struct board_t { + int uid; + std::string frame_id; + BoardConfiguration config; +}; + + +class ArucoMultiBoard { + + private: + + cv::Mat inImage; + aruco::CameraParameters camParam; + tf::StampedTransform rightToLeft; + bool useRectifiedImages; + bool draw_markers; + bool rotate_marker_axis; + MarkerDetector mDetector; + vector markers; + BoardDetector board_detector; + vector boards; + vector boards_detected; + bool cam_info_received; + ros::Subscriber cam_info_sub; + image_transport::Publisher image_pub; + image_transport::Publisher debug_pub; + ros::Publisher pose_pub; + ros::Publisher transform_pub; + ros::Publisher position_pub; + ros::Publisher marker_pub; //rviz visualization marker + std::string reference_frame; + std::string camera_frame; + + double marker_size; + std::string board_config_file; + std::string board_config_dir; + + ros::NodeHandle nh; + image_transport::ImageTransport it; + image_transport::Subscriber image_sub; + + tf::TransformListener _tfListener; + + dynamic_reconfigure::Server dyn_rec_server; + + public: + ArucoMultiBoard() + : cam_info_received(false), + nh("~"), + it(nh) { + + std::string refinementMethod; + nh.param("corner_refinement", refinementMethod, "LINES"); + if (refinementMethod == "SUBPIX") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::SUBPIX); + else if (refinementMethod == "HARRIS") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::HARRIS); + else if (refinementMethod == "NONE") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE); + else + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::LINES); + + //Print parameters of aruco marker detector: + ROS_INFO_STREAM("Corner refinement method: " << mDetector.getCornerRefinementMethod()); + ROS_INFO_STREAM("Threshold method: " << mDetector.getThresholdMethod()); + double th1, th2; + mDetector.getThresholdParams(th1, th2); + ROS_INFO_STREAM("Threshold method: " << " th1: " << th1 << " th2: " << th2); + float mins, maxs; + mDetector.getMinMaxSize(mins, maxs); + ROS_INFO_STREAM("Marker size min: " << mins << " max: " << maxs); + ROS_INFO_STREAM("Desired speed: " << mDetector.getDesiredSpeed()); + + image_sub = it.subscribe("/image", 1, &ArucoMultiBoard::image_callback, this); + cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoMultiBoard::cam_info_callback, this); + + image_pub = it.advertise("result", 1); + debug_pub = it.advertise("debug", 1); + pose_pub = nh.advertise("pose", 100); + transform_pub = nh.advertise("transform", 100); + position_pub = nh.advertise("position", 100); + marker_pub = nh.advertise("marker", 10); + + nh.param("marker_size", marker_size, 0.05); + nh.param("board_config", board_config_file, ""); + nh.param("board_dir", board_config_dir, ""); + nh.param("reference_frame", reference_frame, ""); + nh.param("camera_frame", camera_frame, ""); + nh.param("image_is_rectified", useRectifiedImages, true); + nh.param("draw_markers", draw_markers, false); + nh.param("rotate_marker_axis", rotate_marker_axis, false); + + ROS_ASSERT(board_config_file != "" && board_config_dir != ""); + + readBoardConfigList(); + ROS_ASSERT(!camera_frame.empty()); + + if (reference_frame.empty()) + reference_frame = camera_frame; + + ROS_INFO("Aruco node will publish pose to TF with %s as parent", + reference_frame.c_str()); + + dyn_rec_server.setCallback(boost::bind(&ArucoMultiBoard::reconf_callback, this, _1, _2)); + } + + void readBoardConfigList() { + try { + cv::FileStorage fs(board_config_file, cv::FileStorage::READ); + + if (fs["aruco_boards"].name() != "aruco_boards") + throw cv::Exception(81818, "ArucoMultiBoard::readBoardConfigList", "invalid file type", __FILE__, __LINE__); + + cv::FileNode FnBoards = fs["aruco_boards"]; + for (cv::FileNodeIterator it = FnBoards.begin(); it != FnBoards.end(); ++it) { + board_t board; + board.uid = (int) boards.size(); + board.frame_id = (std::string) (*it)["frame_id"]; + std::string path(board_config_dir); + if (path.back() != '/') + path.append("/"); + path.append((std::string) (*it)["filename"]); + board.config.readFromFile(path); + boards.push_back(board); + } + ROS_ASSERT(!boards.empty()); + } + catch (std::exception &ex) { + throw cv::Exception(81818, + "ArucoMultiBoard::readBoardConfigList", + ex.what() + string(" file=)") + board_config_file, + __FILE__, + __LINE__); + } + } + + bool getTransform(const std::string &refFrame, + const std::string &childFrame, + tf::StampedTransform &transform) { + std::string errMsg; + + if (!_tfListener.waitForTransform(refFrame, + childFrame, + ros::Time(0), + ros::Duration(0.5), + ros::Duration(0.01), + &errMsg) + ) { + ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg); + return false; + } else { + try { + _tfListener.lookupTransform(refFrame, childFrame, + ros::Time(0), //get latest available + transform); + } + catch (const tf::TransformException &e) { + ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame); + return false; + } + + } + return true; + } + + void image_callback(const sensor_msgs::ImageConstPtr &msg) { + + if ((image_pub.getNumSubscribers() == 0) && + (debug_pub.getNumSubscribers() == 0) && + (pose_pub.getNumSubscribers() == 0) && + (transform_pub.getNumSubscribers() == 0) && + (position_pub.getNumSubscribers() == 0) && + (marker_pub.getNumSubscribers() == 0)) + { + ROS_DEBUG("No subscribers, not looking for aruco markers"); + return; + } + + static tf::TransformBroadcaster br; + + if (!cam_info_received) return; + + ros::Time curr_stamp(ros::Time::now()); + + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); + inImage = cv_ptr->image; + + //detection results will go into "markers" + markers.clear(); + + //Ok, let's detect + mDetector.detect(inImage, markers, camParam, marker_size, false); + + for (int board_index = 0; board_index < boards.size(); board_index++) { + Board board_detected; + + //Detection of the board + float probDetect = board_detector.detect(markers, + boards[board_index].config, + board_detected, + camParam, + marker_size); + if (probDetect > 0.0) { + tf::Transform transform = aruco_ros::arucoBoard2Tf(board_detected, rotate_marker_axis); + + tf::StampedTransform cameraToReference; + cameraToReference.setIdentity(); + + if (reference_frame != camera_frame) { + getTransform(reference_frame, + camera_frame, + cameraToReference); + } + + transform = + static_cast(cameraToReference) + * static_cast(rightToLeft) + * transform; + + tf::StampedTransform stampedTransform(transform, curr_stamp, reference_frame, boards[board_index].frame_id); + br.sendTransform(stampedTransform); + + geometry_msgs::PoseStamped poseMsg; + tf::poseTFToMsg(transform, poseMsg.pose); + poseMsg.header.frame_id = reference_frame; + poseMsg.header.stamp = curr_stamp; + pose_pub.publish(poseMsg); + + geometry_msgs::TransformStamped transformMsg; + tf::transformStampedTFToMsg(stampedTransform, transformMsg); + transform_pub.publish(transformMsg); + + geometry_msgs::Vector3Stamped positionMsg; + positionMsg.header = transformMsg.header; + positionMsg.vector = transformMsg.transform.translation; + position_pub.publish(positionMsg); + + //Publish rviz marker representing the ArUco marker patch + visualization_msgs::Marker visMarker; + visMarker.header = transformMsg.header; + visMarker.id = 1; + visMarker.type = visualization_msgs::Marker::CUBE; + visMarker.action = visualization_msgs::Marker::ADD; + visMarker.pose = poseMsg.pose; + visMarker.scale.x = marker_size; + visMarker.scale.y = 0.001; + visMarker.scale.z = marker_size; + visMarker.color.r = 1.0; + visMarker.color.g = 0; + visMarker.color.b = 0; + visMarker.color.a = 1.0; + visMarker.lifetime = ros::Duration(3.0); + marker_pub.publish(visMarker); + + if (camParam.isValid()) { + //draw board axis + CvDrawingUtils::draw3dAxis(inImage, board_detected, camParam); + } + } + } + + //for each marker, draw info and its boundaries in the image + for (size_t i = 0; draw_markers && i < markers.size(); ++i) { + markers[i].draw(inImage, cv::Scalar(0, 0, 255), 2); + } + + if (image_pub.getNumSubscribers() > 0) { + //show input with augmented information + cv_bridge::CvImage out_msg; + out_msg.header.frame_id = msg->header.frame_id; + out_msg.header.stamp = msg->header.stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = inImage; + image_pub.publish(out_msg.toImageMsg()); + } + + if (debug_pub.getNumSubscribers() > 0) { + //show also the internal image resulting from the threshold operation + cv_bridge::CvImage debug_msg; + debug_msg.header.frame_id = msg->header.frame_id; + debug_msg.header.stamp = msg->header.stamp; + debug_msg.encoding = sensor_msgs::image_encodings::MONO8; + debug_msg.image = mDetector.getThresholdedImage(); + debug_pub.publish(debug_msg.toImageMsg()); + } + } + catch (cv_bridge::Exception &e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + } + + // wait for one camerainfo, then shut down that subscriber + void cam_info_callback(const sensor_msgs::CameraInfo &msg) { + camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages); + // handle cartesian offset between stereo pairs + // see the sensor_msgs/CamaraInfo documentation for details + rightToLeft.setIdentity(); + rightToLeft.setOrigin( + tf::Vector3( + -msg.P[3] / msg.P[0], + -msg.P[7] / msg.P[5], + 0.0)); + + cam_info_received = true; + cam_info_sub.shutdown(); + ROS_INFO("Received camera info"); + } + + void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level) { + mDetector.setThresholdParams(config.param1, config.param2); + if (config.normalizeImage) { + ROS_WARN("normalizeImageIllumination is unimplemented!"); + } + } + +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "aruco_multi_board"); + + ArucoMultiBoard node; + + ros::spin(); +} diff --git a/aruco_ros/src/single_board.cpp b/aruco_ros/src/single_board.cpp new file mode 100644 index 0000000..3ac7315 --- /dev/null +++ b/aruco_ros/src/single_board.cpp @@ -0,0 +1,355 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +/** +* @file single_board.cpp +* @author Sarthak Mittal +* @date April 2020 +* @version 0.2.4 +* @brief ROS version of the example named "simple" in the Aruco software package. +*/ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace aruco; + +class ArucoSingleBoard { + + private: + + cv::Mat inImage; + aruco::CameraParameters camParam; + tf::StampedTransform rightToLeft; + bool useRectifiedImages; + bool draw_markers; + bool rotate_marker_axis; + MarkerDetector mDetector; + vector markers; + BoardConfiguration board_config; + BoardDetector board_detector; + Board board_detected; + bool cam_info_received; + ros::Subscriber cam_info_sub; + image_transport::Publisher image_pub; + image_transport::Publisher debug_pub; + ros::Publisher pose_pub; + ros::Publisher transform_pub; + ros::Publisher position_pub; + ros::Publisher marker_pub; //rviz visualization marker + std::string reference_frame; + std::string camera_frame; + std::string board_frame; + + double marker_size; + std::string board_config_file; + + ros::NodeHandle nh; + image_transport::ImageTransport it; + image_transport::Subscriber image_sub; + + tf::TransformListener _tfListener; + + dynamic_reconfigure::Server dyn_rec_server; + + public: + + ArucoSingleBoard() + : cam_info_received(false), + nh("~"), + it(nh) { + + std::string refinementMethod; + nh.param("corner_refinement", refinementMethod, "LINES"); + if (refinementMethod == "SUBPIX") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::SUBPIX); + else if (refinementMethod == "HARRIS") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::HARRIS); + else if (refinementMethod == "NONE") + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::NONE); + else + mDetector.setCornerRefinementMethod(aruco::MarkerDetector::LINES); + + //Print parameters of aruco marker detector: + ROS_INFO_STREAM("Corner refinement method: " << mDetector.getCornerRefinementMethod()); + ROS_INFO_STREAM("Threshold method: " << mDetector.getThresholdMethod()); + double th1, th2; + mDetector.getThresholdParams(th1, th2); + ROS_INFO_STREAM("Threshold method: " << " th1: " << th1 << " th2: " << th2); + float mins, maxs; + mDetector.getMinMaxSize(mins, maxs); + ROS_INFO_STREAM("Marker size min: " << mins << " max: " << maxs); + ROS_INFO_STREAM("Desired speed: " << mDetector.getDesiredSpeed()); + + image_sub = it.subscribe("/image", 1, &ArucoSingleBoard::image_callback, this); + cam_info_sub = nh.subscribe("/camera_info", 1, &ArucoSingleBoard::cam_info_callback, this); + + image_pub = it.advertise("result", 1); + debug_pub = it.advertise("debug", 1); + pose_pub = nh.advertise("pose", 100); + transform_pub = nh.advertise("transform", 100); + position_pub = nh.advertise("position", 100); + marker_pub = nh.advertise("marker", 10); + + nh.param("marker_size", marker_size, 0.05); + nh.param("board_config", board_config_file, ""); + nh.param("board_frame", board_frame, ""); + nh.param("reference_frame", reference_frame, ""); + nh.param("camera_frame", camera_frame, ""); + nh.param("image_is_rectified", useRectifiedImages, true); + nh.param("draw_markers", draw_markers, true); + nh.param("rotate_marker_axis", rotate_marker_axis, false); + + ROS_ASSERT(board_config_file != ""); + + try { + board_config.readFromFile(board_config_file); + } + catch (std::exception &exp) { + ROS_ERROR("Could not read '%s'", board_config_file.c_str()); + exit(1); + } + + ROS_ASSERT(!camera_frame.empty() && !board_frame.empty()); + + if (reference_frame.empty()) + reference_frame = camera_frame; + + ROS_INFO("Aruco node started with marker size of %f m and marker board to track: %s", + marker_size, board_config_file.c_str()); + ROS_INFO("Aruco node will publish pose to TF with %s as parent and %s as child.", + reference_frame.c_str(), board_frame.c_str()); + + dyn_rec_server.setCallback(boost::bind(&ArucoSingleBoard::reconf_callback, this, _1, _2)); + } + + bool getTransform(const std::string& refFrame, + const std::string& childFrame, + tf::StampedTransform& transform) + { + std::string errMsg; + + if ( !_tfListener.waitForTransform(refFrame, + childFrame, + ros::Time(0), + ros::Duration(0.5), + ros::Duration(0.01), + &errMsg) + ) + { + ROS_ERROR_STREAM("Unable to get pose from TF: " << errMsg); + return false; + } + else + { + try + { + _tfListener.lookupTransform( refFrame, childFrame, + ros::Time(0), //get latest available + transform); + } + catch ( const tf::TransformException& e) + { + ROS_ERROR_STREAM("Error in lookupTransform of " << childFrame << " in " << refFrame); + return false; + } + + } + return true; + } + + void image_callback(const sensor_msgs::ImageConstPtr &msg) { + + if ((image_pub.getNumSubscribers() == 0) && + (debug_pub.getNumSubscribers() == 0) && + (pose_pub.getNumSubscribers() == 0) && + (transform_pub.getNumSubscribers() == 0) && + (position_pub.getNumSubscribers() == 0) && + (marker_pub.getNumSubscribers() == 0)) + { + ROS_DEBUG("No subscribers, not looking for aruco markers"); + return; + } + + static tf::TransformBroadcaster br; + + if (!cam_info_received) + return; + + ros::Time curr_stamp(ros::Time::now()); + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::RGB8); + inImage = cv_ptr->image; + + //detection results will go into "markers" + markers.clear(); + //Ok, let's detect + mDetector.detect(inImage, markers, camParam, marker_size, false); + //Detection of the board + float probDetect = board_detector.detect(markers, board_config, board_detected, camParam, marker_size); + + if (probDetect > 0.0) { + tf::Transform transform = aruco_ros::arucoBoard2Tf(board_detected, rotate_marker_axis); + + tf::StampedTransform cameraToReference; + cameraToReference.setIdentity(); + + if (reference_frame != camera_frame) { + getTransform(reference_frame, + camera_frame, + cameraToReference); + } + + transform = + static_cast(cameraToReference) + * static_cast(rightToLeft) + * transform; + + tf::StampedTransform stampedTransform(transform, curr_stamp, reference_frame, board_frame); + br.sendTransform(stampedTransform); + + geometry_msgs::PoseStamped poseMsg; + tf::poseTFToMsg(transform, poseMsg.pose); + poseMsg.header.frame_id = reference_frame; + poseMsg.header.stamp = curr_stamp; + pose_pub.publish(poseMsg); + + geometry_msgs::TransformStamped transformMsg; + tf::transformStampedTFToMsg(stampedTransform, transformMsg); + transform_pub.publish(transformMsg); + + geometry_msgs::Vector3Stamped positionMsg; + positionMsg.header = transformMsg.header; + positionMsg.vector = transformMsg.transform.translation; + position_pub.publish(positionMsg); + + //Publish rviz marker representing the ArUco marker patch + visualization_msgs::Marker visMarker; + visMarker.header = transformMsg.header; + visMarker.id = 1; + visMarker.type = visualization_msgs::Marker::CUBE; + visMarker.action = visualization_msgs::Marker::ADD; + visMarker.pose = poseMsg.pose; + visMarker.scale.x = marker_size; + visMarker.scale.y = 0.001; + visMarker.scale.z = marker_size; + visMarker.color.r = 1.0; + visMarker.color.g = 0; + visMarker.color.b = 0; + visMarker.color.a = 1.0; + visMarker.lifetime = ros::Duration(3.0); + marker_pub.publish(visMarker); + } + + //for each marker, draw info and its boundaries in the image + for (size_t i = 0; draw_markers && i < markers.size(); ++i) { + markers[i].draw(inImage,cv::Scalar(0,0,255),2); + } + + //draw a 3d cube in each marker if there is 3d info + if(camParam.isValid() && probDetect > 0.0) + { + CvDrawingUtils::draw3dAxis(inImage, board_detected, camParam); + } + + if(image_pub.getNumSubscribers() > 0) + { + //show input with augmented information + cv_bridge::CvImage out_msg; + out_msg.header.stamp = curr_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = inImage; + image_pub.publish(out_msg.toImageMsg()); + } + + if(debug_pub.getNumSubscribers() > 0) + { + //show also the internal image resulting from the threshold operation + cv_bridge::CvImage debug_msg; + debug_msg.header.stamp = curr_stamp; + debug_msg.encoding = sensor_msgs::image_encodings::MONO8; + debug_msg.image = mDetector.getThresholdedImage(); + debug_pub.publish(debug_msg.toImageMsg()); + } + } + catch (cv_bridge::Exception &e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + return; + } + } + + // wait for one camerainfo, then shut down that subscriber + void cam_info_callback(const sensor_msgs::CameraInfo &msg) { + camParam = aruco_ros::rosCameraInfo2ArucoCamParams(msg, useRectifiedImages); + + // handle cartesian offset between stereo pairs + // see the sensor_msgs/CamaraInfo documentation for details + rightToLeft.setIdentity(); + rightToLeft.setOrigin( + tf::Vector3( + -msg.P[3] / msg.P[0], + -msg.P[7] / msg.P[5], + 0.0)); + + cam_info_received = true; + cam_info_sub.shutdown(); + ROS_INFO("Received camera info"); + } + + void reconf_callback(aruco_ros::ArucoThresholdConfig &config, uint32_t level) { + mDetector.setThresholdParams(config.param1, config.param2); + if (config.normalizeImage) { + ROS_WARN("normalizeImageIllumination is unimplemented!"); + } + } +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "aruco_single_board"); + + ArucoSingleBoard node; + + ros::spin(); +}