From 6d0530c29298fc7a6f20f25619c5a106d8d9b6e5 Mon Sep 17 00:00:00 2001 From: EnricoGrazioli Date: Tue, 28 May 2019 16:12:40 +0200 Subject: [PATCH 1/2] Roba. --- src/odometry_node.cpp | 50 +++++++++++++++++++++++++++---------------- 1 file changed, 32 insertions(+), 18 deletions(-) diff --git a/src/odometry_node.cpp b/src/odometry_node.cpp index c89dd00..0d6ae0e 100644 --- a/src/odometry_node.cpp +++ b/src/odometry_node.cpp @@ -30,6 +30,10 @@ #define DEFAULT_INIT_VELOCITY_X 0.0 #define DEFAULT_INIT_VELOCITY_Y 0.0 #define DEFAULT_INIT_VELOCITY_ANGULAR 0.0 +#define DEFAULT_INIT_VELOCITY_LEFT 0.0 +#define DEFAULT_INIT_VELOCITY_RIGHT 0.0 +#define DEFAULT_INIT_STEER 0.0 + #define DIFFERENTIAL_MODEL_MODE true #define ACKERMAN_MODEL_MODE false @@ -50,6 +54,9 @@ typedef struct last_odometry_data { double linear_velocity_x; double linear_velocity_y; double angular_velocity; + double steer; + double velocity_R; + double velocity_L; } OdometryData; typedef struct diff_drive_control_variables{ @@ -86,16 +93,20 @@ void configChangeCallback(odometry::parametersConfig& config, uint32_t level) { } void differenrialDriveOdometry(double delta_time, double speed_L, double speed_R, OdometryData& new_odometry_data){ - + + double linear_velocity = (speed_R + speed_L) / 2; double angular_velocity = (speed_R - speed_L) / BASELINE; - + new_odometry_data.angular_velocity = angular_velocity; + double delta_theta = angular_velocity * delta_time; - - new_odometry_data.angular_velocity = angular_velocity; new_odometry_data.theta = last_odometry_data.theta + delta_theta; + new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta); + new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta); + + if (new_odometry_data.theta > 2*PI) { new_odometry_data.theta -= 2*PI; @@ -107,46 +118,45 @@ void differenrialDriveOdometry(double delta_time, double speed_L, double speed_R { new_odometry_data.x = last_odometry_data.x + (linear_velocity / angular_velocity) * (sin(new_odometry_data.theta) - sin(last_odometry_data.theta)); new_odometry_data.y = last_odometry_data.y - (linear_velocity / angular_velocity) * (cos(new_odometry_data.theta) - cos(last_odometry_data.theta)); + } else { - new_odometry_data.x = last_odometry_data.x + (new_odometry_data.linear_velocity_x * delta_time); - new_odometry_data.y = last_odometry_data.y + (new_odometry_data.linear_velocity_y * delta_time); + new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2); + new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2); } - - new_odometry_data.linear_velocity_x = linear_velocity * cos(last_odometry_data.theta + (delta_theta / 2)); - new_odometry_data.linear_velocity_y = linear_velocity * sin(last_odometry_data.theta + (delta_theta / 2)); - } void ackermanDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){ + double linear_velocity = (speed_R + speed_L) / 2; double angular_velocity = linear_velocity * tan((steer / STEERING_FACTOR) * (PI / 180)) / CAR_LENGTH; - + new_odometry_data.angular_velocity = angular_velocity; + double delta_theta = angular_velocity * delta_time; - new_odometry_data.theta = last_odometry_data.theta + delta_theta; + new_odometry_data.theta = last_odometry_data.theta + delta_theta; + new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta); + new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta); + + if (new_odometry_data.theta > 2*PI) { new_odometry_data.theta -= 2*PI; } else if (new_odometry_data.theta < 0) { new_odometry_data.theta += 2*PI; } - if (angular_velocity > 0.0001) //exact integration assuming velocities constamìnt in the time lapse { new_odometry_data.x = last_odometry_data.x + (linear_velocity / angular_velocity) * (sin(new_odometry_data.theta) - sin(last_odometry_data.theta)); new_odometry_data.y = last_odometry_data.y - (linear_velocity / angular_velocity) * (cos(new_odometry_data.theta) - cos(last_odometry_data.theta)); - } - else //for low values of omega I use the Ruge-Kutta approximation - { + + } else { new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2); new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2); } - new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta); - new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta); } nav_msgs::Odometry populateOdometry() { @@ -235,6 +245,10 @@ int main(int argc, char *argv[]) last_odometry_data.linear_velocity_x = DEFAULT_INIT_VELOCITY_X; last_odometry_data.linear_velocity_y = DEFAULT_INIT_VELOCITY_Y; last_odometry_data.angular_velocity = DEFAULT_INIT_VELOCITY_ANGULAR; + last_odometry_data.velocity_L = DEFAULT_INIT_VELOCITY_LEFT; + last_odometry_data.velocity_R = DEFAULT_INIT_VELOCITY_RIGHT; + last_odometry_data.steer = DEFAULT_INIT_STEER; + last_msg_time = ros::Time::now().toSec(); // Config server init dynamic_reconfigure::Server config_server; From df64718b1e1a98b0aaccd468a4ea091ef1989d6d Mon Sep 17 00:00:00 2001 From: EnricoGrazioli Date: Wed, 29 May 2019 19:03:08 +0200 Subject: [PATCH 2/2] CustomOdometry publishing added. --- CMakeLists.txt | 2 +- README.txt | 37 +++++++++++ frames.pdf | Bin 0 -> 18262 bytes msg/CustomOdometry.msg | 2 + msg/OdometryStamped.msg | 4 -- src/odometry_node.cpp | 142 ++++++++++++++++++++++++---------------- 6 files changed, 125 insertions(+), 62 deletions(-) create mode 100644 README.txt create mode 100644 frames.pdf create mode 100644 msg/CustomOdometry.msg delete mode 100644 msg/OdometryStamped.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 641dd2c..8756817 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,7 +59,7 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files(FILES - OdometryStamped.msg + CustomOdometry.msg floatStamped.msg ) diff --git a/README.txt b/README.txt new file mode 100644 index 0000000..78ead23 --- /dev/null +++ b/README.txt @@ -0,0 +1,37 @@ +Project team members: + -871569, Grazioli, Enrico + -868454, Finazzi, Andrea + -865784, Colombo, Marco + +The archive contains the ros package directory "odometry" structured as follows: + -CMakeList.txt: automatically created by ros. It contains all the configurations + used by ros to build the executables of the package such as dependencies, + packege files names etc. + -package.xml: another ros generated file with similar porpouse to the CMakeList file. + -frames.pdf: scheme of the tf tree. + -[dir]msg: contains .msg files used to describe custom messages. + -CustomOdometry.msg: message type published on "custom_odometry" topic. + It contains Odometry.msg information on computed odometry + and a strig specifing the odometry model adopted. + -floatStamped.msg: message type used to read messages generated by the .bag + files provided. + -[dir]src: contains the .cpp source code files + -odometry_node: it's the only source file of the packege and contains the + odometry computation function and all the subscribig/publishing + logic. + -[dir]cfg: + + +Configuration.....finaz + + +Custom messages: + -odometry/floatStamped: + std_msgs/Header header + std_msgs/float64 data + + -odometry/CustomOdometry + std_msgs/String odometryModel + nav_msgs/Odometry odometry + + diff --git a/frames.pdf b/frames.pdf new file mode 100644 index 0000000000000000000000000000000000000000..efa1d41c7e28298e008894888207f05bafd5e947 GIT binary patch literal 18262 zcma%iV~{A#vhCQOJ+^Jzwr$(CXZF~(ZF`Sx+qV7ockVei?!%AQ(b3hpGOMzxBG!uN zs7?}jVNn_eS{5jh=Bt_yC}w$ z>ntn96#a0jT^nl3U?jIFKGqz2|E83kBe`ScUfMrJY##QJg>>|k;?xTAPZzioV9kmq z*2(zy5(thbUC@{=6le#R+HR+qiQr52atc&lr-FV5 zX!mMa`rX=zYQRNNhQMV^vO^nr$&%M^ZKW127f!xkcqi4Yed|%rKH{Tdi;hH@$?u1^ z9C_)fgd@^+4b-Z3^PWx z#ux=dvnv)oq=wVxyBPHtjG1mBW~Vv2@}m0I_w$xYr-smZ^C6%bh3)K~rY-X1UE$Us zI$t7Mvn6pNDH0_KB1lB0^f){t7VwNESCPG|0xx*V3jThHWq86f1>-Ojl%B>a_42i& zpHGT<8VubrzzIVn0ME?F<&oWytTXj;s9Ie?9#pVT3EqVJR1u-p)e1}w7gw6P;V0kV zHwXRdas~)H-09%XEw2E&YO?AAL{q53FiOs9@N`@^i4vtwr_c73+ex~k@z{?=xAaU? zYD@7#)9I#LrMuGN>HtVkW!-NWgrEGWL%<=9oTv;3nn&`%NdZk(!AsM2n6h&m_b<$k zmv|PI7lyL;8hfCRVRw^)c3^V*TcFOt%_tq1ELsTe2k{8^QC8Kw4*NpVCfZBF-WZO$ z(}^6LhM1J9xJa;=$gs+qmw_4H?({R1OUy9U^L98ksSFmsZ<5QLl!zLfxJZqNK09*I zulL!8n)Xai3tNsctf-ql98WT}t*p#t+h_@?imE7`(3m&O@=fGceoHm4s@*CQwWaEN z9ViOB?C~5`zvwg7+@CP2u+G6JGTseoOmK}^m8Pez0(aBA`8`vcW@-0FrbKMzq;EDl zLi!;30qTxruS1#G8vmOKf8YM$&_Bfar~OMEHdeNO{Qp=?|IM&}aZ1G9Sxm|KFQ4$a zx&HzFUUUZZe+_&(Apv{_d^#h8zt6uc`|J3xC!MIBt@HngHl?Abp~q+a4@>_B{k{L4 zjPbvd;nOL5*qh+f$s3rN;FI7p{42Se!oY~n z#7d8^^RH;3zj^!N{sT+k zJ3G3V{Kpdd8zpSwYGGueC?@z1`0u4EnmF0HI2xHa;s3L3ivMF}Y=7fr*WS<9{Gb_$Er{&kI;#b37z;uZ)N*x6}i4|~0Q>2uAqR(kWL*RkX2yo9d8a?-l|X*{LE;_m-j zS|eIwadYk$`OztWp`nrK5jeTVMrKEQ1?M^y$2|poIS|bjzzsJW5K3|f%P8DyFAaa* zJV-~ICKu=J&x@i1S{~$Sb{sum_l$&v996g>kQc*ak0zJ(!p6R5bxh6Wc0M+dO^(Al@r zQMWv6$qO1S2&F&sEa2(=_WTak4`zB&VI;}zjjp=KDhEK5Dt;U?A9Uo9*XVvNLSrRp zBQV0?eT@XknLh2c`Hvb)4FrUrO-h|BU`99F&Cq@CW%no$TLNa;l`u zd;$y%VsifQ$XIN2{t?-!uzkbt+n9JQy?$RXhR5yyiXr|0k2m%T>(;15626NG2_$L6+I`jGl3`(SBpZE5ZVbv;-K z*^$t@17PmG;`}qcH$eU&YyF6iJ)paXVE4dXJ37I=Fn#@S`VKCP9(-GF?{=YyD1H0J z`#{chz?%Nwb=E&a-xpawGETh<{AgBU4}8C7It#KRDwps=(fambogF@fZUNP?alVVZ z?P5R=@5sJ=7pd{7(02J$yHYLoPLJ=ycHPOUwtngZA9$`lrast?Jx6X_on8mJR-is( z&)c`9TBXJDZa7pdPbCgqF=XFHorNQ z)tJOm`OaRqThl%TZpLJO@?J2~zp%cJU{N3Z`d%Obe*k|lXZUtM?5TeMcf^dI;9qs4 zp7?gX5L@iR3$aBDITmqB;7Q-SNIpLuQN^Gh7m88J zfj8XKYoE3wf`rCWkeqxh5(iY=BC$0p(QDk$|JvWTQkp2J`74f**Wpz65ptU#Q)%WP zc6E5r%kgZxC1aG$;P!tSNjJ1|D4joKj~kGFDL50e=>lu0n|YP&PueAzBcoLCg~g47V?b)9t|E@)lCuME_*LO1Bl??gWW!QQva-19L+P`r9y6 zM>HP(aYj}5;Pb8Pq!voqeEH&4>zf*rhwZe1ur|zx!!QyBUS3&WVj7oQUjrZO;PL8% z(7{Y1lQ6{Qc~D*6Jy=gISIpBaC*q+YHE=LX<8GN5vDOd6N9hN9AW082F+#H$7RFv4mJ z1GGY{k1kq#k`*elO09vczW@@zEQ-T1wW77EEq+b!!`IUbHO+ItN$G*IF$y-ugH8a z+C1ab!N_&?!0sbC*UVtI3Wf8=?2m6{8ibomL1DH!9|mzz*y7ta{{oJ&y|i4OqD0B> z!K4IaWPEdr(2$h|U1BJX!@WOFWSF_FOd#che&5*7=B`$TMH`zJ?|TOwc1*9~!Su#A zOV~sXEOJy`mcgSrR{M4|9|VM)idF0-R;=O`sm)HNPmkXAxh8Z6cZH?oHU$kK<-mT8 zB0>3=Y-4B}X_p9`TH>qo;YW%+^cK2*3L#mx(^uK+PEB2Roa+v~PFtCrG!A2lIpRwv zULME=mgaSbc=j-(CUsK1c{k6f9iAe#fSkxFd~?gG?sGsJ;ya{ETh(Rhp=x^7TUK#rejN0gTCTZK5@c?bp60ur6|J8+TC^;OmEqE* zpM%{&EAr)c9=+G*?7ECBtRVHL%#GUO1#eX=vkJxerp{lUm#Z_F+-MLSBM}RmZ+9s3 zI4C~x&Y8N8GIph1nWzDq`G*{)fui!4Xhwz{WQ^9r%oDipgd@)MCFC)!W$X-*YAO9{ zuyu2NGQlkp7g~5?1%@VS083Pxe%p9{B?xYOuO01O!Oo~*W0z4 zDxY(E^*H`+Z&X_aFATGMY%VB1h7U zJ!y&FU^Gy!!G1|`r8*arE!R|;2Y;H@IzWO(67PEd0L7%}W3z-MxHY`mmZm$P`r8B} z97t{xEt+bIxl_;QY)X*qn{FJW5_(oa6rI6QlNz_)2aLL5Cx7=b_u`+P zluYQuF~Lk8C{e~4{pOILN%{3VCrE+V16iAH%+fL_=GMc`t*>;-0zdR9Y}Oj`h7*Y? zYw~i7JgtK{{h4#Lu1&K2KP)S=z^gE5Jo6N-jbYR>JGXs0`Z+fg^y#Ga>bVlU6MY?T zt`r4rTRqO;6{%G@eaMO0yE*gU?JnPl^+jh<_-vZ0lGf33zZQ15pWc@jx>&z-n}waMHGot(!% zFgCS341VKCH7MzI1>V9(4;D&7?GJI)hLDHpXSj#%ch_6s8ZT%#cF!JnP!`B%^Ny%g zPGjQG=gFK}5X!9U`A$C|y^R-3+188LHG4AErZF332MsE7j`tlP9%hcLOJ#8|H2PJu zkAe;8hfQFqm~N0J&iGRx1H>aRnU-$Y$i8KCq(qsY?piMqJfQ*(uor4pYYXnC+sp9+ z$J;H*g5Gq!Y&5GlTwjjCU}$)}VzF3`LGvJ2J!M3VJ|15VhW15Z=VizfJz25Yel}t= z!6~iNXxa-+_!=NO!u(RQv=V+B!|>e9Nls+~+45EqA=po!-vybp{gjT+LsfBW+EaXU z+@SqbHdD*rxG3gd7TyH+<>=>~>8Va!e?2Y5YD4lz)EsG`sD8#eeZZ_y=ZeXiRM2WVZ5Jfq> z`~1N;4F6orW(>DT>;=?_QF?(pe#}P`MQ4k?8!)1JduoSzHE*98SytoPcEq9hxiyap zF%?zRv*R-BTXw9kc1sUdN|AGc@0AAmZokJBq;Jltp12UJ8jJ0!Z=`?JaZdigCiWrg zND-Jkxb`4|K1Q>0#Nvw6#_*uHNF4@Si;j9jUCi0bq@N(3V1v_wZ!^res)ylj@bA}= zpuP!rXUhy*?raZ(#UkxJ>ve1- z4`LvsU#;XDZnE>Hj46&^zh7P$8x!?MViH-A5-)FUNqXUxwcSu#8pBlarDOpoO7|9O z4m3+bn82#a_8lN)vAFp#M2Jg;4kq_m>+~>ldIAkf8x0>>7GC|N7gY}r3^PYVzM+ge z@h~x}1*ne)xl+TREtzq(!r*4$^5FpN9yRFH&`d3#D6ceIFN zW4KR_f>2VxRjApG1zS%G$xWs=v*@rC4~8w5kD243!7l>c)61mzTikUjQmp&saRx;;4(<{6-!ns2^=on%E zH#qcc0#iu=c^2feL(gU~t`&*TwWAfmN?kqutGY6_%i8VzvyIKVPlW-g5^900pdY@t z({-)K(1cuuI5-rXqO4F-`kT-j;s z%kg9yI*yu?TYpI;N`&(n|88Af5IJRE>yQd2$8yV)tT;k>#Mi$-oEL$MG|G$1&~*f^ z6eC|gRoeRVdDcAY`8b~N3!j{>fM3KQl7aDps=`gZx_$Z0-TDSoBJkq(g?UEB>CTW0<`nDj;-&n4hBS~xmfuob3?sGIjuGbq0K>xjWrIYgi`dtl|Xti zB^}n?0WnktVlle!op3ZwpIR zOa>ot8+qco0X7eXvlN`z4)-5h>`blkIszLt&C?=-VNrckc~TWJYQr1MfxH!A!OWda zWDMbsN3A?Tepa4!RmhR4gj)6jg*nzR@Jz)IjN4}Swii>q76);f+(2?n-7z9Mp_v|t z^Z~e)864guP@Cl6+tS6?Dkvt{yboVT|CB57_Nfvj{o$g$Q!Qfdfhe; ztem)WECNSmv{3Bmn%8@wB-NJaBAicetNsd?4i4amGv3;hf#T%890Yk*9Z%4vd2mU@ z$`2WS6S~-p>KAypB^G;`gl+g&+tAzMc#Sm{YD=L@(Rqa|MXt!FWy})U^%2Pq{=Vuf z=)_KW(XN7!vsX3*7ze+k!fH6{=g49OlhrHiIIoB*{J5l9$xubpX4s*KO8mFd6_74L z|I9B80fmK3o1!f}N5WrZ>V|7#owPmcnt!%t)#F8-D`WEG9ASfJ7R015<%)8tC+if= zWkIHb~K; zBepti#ZiYt0{mDw_OC5OX~R^}c3$93e>|c(g4Pf6@l8$>{q!N@BxsK+wW-kC>?pn! ze`SfIxX*_RLgkn&pf(X*g6ihIFj=~mxul*>)e>Ik(>n+A`#z%6Qe{f2y>W9p76`N< zAD>$_an|@LW_@?B*HwIVYqUDfQ&C28F5=t?6T0Wl=l7jq1Y+D6rzy(KMxp+OAy~B# zWj5+{KjClMq-r>2e)DBb3%P?ViNn6K#^r>dPQrN7$ik9|@NbbKqKrfjJe)sHR1g#P{RTO1KAFVAcpm8To_KD@dZ(o~RxR%1Ou z3$r?;Nn9exhJR@?MV$-LKooctJ`L@LGj?h7!8CkY`^=(nC$<5%fSF=M&%rc^Q|zpZ*16%ohx#7i*3d<;CR*WRX|YgzzYJA1`W4}?JgjQZ_QC3OXGN}IDA6& zT9kMNg%SM*Qc)OFJ|80=>uTt)XBarZ-BWCx)YD&$#b_j5K$MfnXk@`yXcS$4UdObj zT<62a{w-Z5Ozh3c5ND2pK|CPYLi@Xpy06leblW{(*xoBZn2?sisbmnA)Q*YOIF4d{ zc{>>!tOgRb4Re{3b_XDged)O}+eiB8w9?LfvQc$LN517(Xmq#I`VZ znsJY?;T{iPL^XUFhPOvGb8GvRoQR32kcdaZ4(!VqCeG_1(po>$Dac{9NR%a zN=HLF`?+y2`#|pUT_g!#SzTSrSDJCN2>Q*?LoYBIma$zLY2V3=AXz$*wX1K^CT>a5 zSPV5JJ&qbH&C;vsr-@Lh$B~JrJ;2onhId{~hTQp_D^j*!uBu5-dI8$b@94Y-w9F{r z49L+M0=z{GUU4F%w5>Gl;3#Af91{~~W4pAvTs<{AETQqSmHMh&melir1d{H`^N^DY z<>dT2iD$e%#Me;A++d`DOIg?IEQtfcwCXvu>FwEGz0wRKcdh&*{e30X>W}pP5OHfP ztjE^14?%VrFrIl2(80NMn)Hh0Qk2u^p6F(TQt-VKAGRIKje@x_rwM>rTBiHvZ=O-| z0~^nICU@%4f*u22;orHIp1I+9+A*luuoMjB;8nQ9i%iuwm@`xn2>?A12YDFj53FPJ z47qO}*|=92Xb}nH;wfc|#0^47BMy!zs_xZm30`7x0}$)IxLjE?S?s#Ed5fdwaMrny z^b1cEpw6$D?H%i!Lf$Edb2txV*eMdzs9L4^{85;?DHQpV-7VpRIPSBMGEj#S$ySV& zCU_kK656u$JID&dBLUX%3t~)HIvCmT3*(Np-jom5tGo@>^~bSFmU0_}KlLr=h48G} zC7R`DZ z>k&ayweJDy>hgt$;Ykr0?_h*UbSK)frK2R!&d}ggpjH;AgC(OIs4KptIw4(=YvI`< zR0q-K9SLDt$&$wrjy7(y+RD2~Q`NMz>C~OW!x*U!-BnsD}^JDpDtahzj zg8rMP+rI{!2!KTuO2h!sZLTB-`CC8OeryBbCDi0?9Do2JlJ^dPE)R-2Rj*jU)9ecM zfDownH}EQ-cl?dEG*Y5)i15usdACL9PA}+rkac2>t+(R@jjl?Q4;XfD8vxA^>le}@ zp3rPf^=YMtYthI~!2 zVqCEkg8^ARI(dydGWCvf@IqMcN==TN)#53&1b2nlLB(z4*(N5@!%$y@vkYB%p)s#3 zUYz_^8CU6%cAw?+7p`d$pLF`YLltHiSV;9qm=14S3SaSUn)v>h{_1DVZ7dJ?;9mNo zA^kNQHK?;|F1ln=rufdJ;wQBM+hqk3(ql=WIYLraYL*LocIE9Q58W0N96ulSqx7p|leY8rb;PXg^&V!AdacD;B<;(U%L*yY^`)xuBxps`$!3JcOt zjnTcW-!*VLYe7V&8SO{Lg9dvBnIr{)0Z@AgZgv+T_4kP3@-8lu@|3^kx3}IRWUuan zE%}QPM1e-~uIiFey{zAb;q+?@HsE2 zO;)xj<~iuj7)fdjrT}cG{{!)&b5KmJC`$fHSQ|^Vx+e-dclmoyxFN5FAsKn*I2y{x__4NOxch>L$FW&%hqRg7+5^&5eVK?G?1z zb{T&(<`eG=@5?;%1sm~Q@%-mi>k=!TW;Z~s5Y|gd*>kJX4(npPaiMNlwrsb-qHFkh zq2RSHpdPrzNaS$nGG5uxvR#-v*z#mlqabE>*!lA}ApHUDFt(Bw>0|aFP3HTVswM5L zA$cTG6qJl8wl04Z>*8y@`Dqx-<$z1+#gJc7taFtCHoWgfSn~|pUgoQe@iN=x0rpu4 zI4%eLtn2bIyhF_fj);4S%o%#M;lmVqTSs30x8b8|g2k0AavvP9`3GK?gW%Mx`MC%h z)z&0kl~S5xu8t}`@-rNqrv7f>!QCt1YBI#tV`!!(rSHc}T{gH0aebY`56s z;~EqHb!1$x`3#74$K%)B}Z>3 zp8&-(o))cC_A+Na6fIivlvBlCLl0~^m3HGA`dyQtg&2nyMYtU%bh#X+khi<;C&Y=R zT~(&TEAT`txzB#7^Pbl2VL#~lf*5CCv^Kou+pp3q05?oM1mEI4dYZ&*0&4ej# zI1y;_t9%ojpm?P=SX8f?>Q^lrL*w2AuAi`k1*&r}e8?E?MJY3`tE*jEFaaMTLV6R! z^4f{+i!AoPy&0>dP<#F91{e>vl79lFA9#LA%O56YzC^2TN0t&MO~toQ^X><4*o&s> zprT<1MuuE$EN``{&Q)Tos3VajwVNYuLcjTpusZOKlt@uD*3kaxwwx!W0tuoWO^v8# zqEh?Ckb;=zT%?B^y&t2C`|I!wppBOq^Jh1~^ox#>5_)tBMe4VU%AhwRn&!DSSFine zUZSMEO!qDQuWl;qb14CYWEEQ`$Gz+uqz(vF!;7)g)(LXWCZ|f)cI_<$ZF*;4q(D7m2!z7uYje|ty_7{`zW<<;?MWE8IH$7lC6xgE$P~0}4TsI(3f}u?>03veM zvUrf*Bo?WD5=$mIgfYZC9t9kqKBvkADAn{XgsY_%f2DOUX<^td!$8D=EwSkV> z+%sFrmjpGvYigO~4#03jmCf0@FY-Tr}c;MMPWtGBi6uueEDqgM-q6Nd^B+n zUlD;TR7%a6C5IE?rn`~xQ?nV9@S5q~DQqoBBGmX&BzQ5n@QfweowuR2rLqwf}@}mX{)MC zt}}XGiwLbR4eljH!uB%`_`3prbOCOfKO`eRcF0Bt=kJ@P>Yet3nVJ$lq}2{vXjzq@ zQug1Ew1!W{ayX7wf@9DkV35*>1ZNep8DM!gT5E6gMiF(OEN`5v=8}YX_Fk)hwBR(g zHR%QeALT%@zD?RBQAgm<2-bubWJ+@R1#PjozT#cq>idax;&}1kE^rxX#EETLBO_)e z4B64$3!A5QvJ|GMlAV0P)uIuJLpki{xxf4DV@C7`PK85>w@+dDII;_aZ_uwTFUVaU zRE2*nAlaulN`c5LOf*XbDgX%#V%l?uy@$2<_o&QFXcS1`M7!{_h0?GRz;t%`Mr z=WKQ^aF{yK&C{Ph!=BqEaD2&a^9qs26(NO!6;;ESaCWvT5VP^B3Db+kufln7q{Dg4 zF2iLJLy21w{Hg5LcHuThm6#S3a34m|Bmvhsz(3VZjLqoY-N7NOBw1=FBw9hOk8At= zd8A-4LSm6qusPnQBOQyR;!2@Sg#BSA)wZ+lBR-*Hn^@%n}UR0GMhB|zLy4bPrL{tj0{}IBXtXmVI5*W5egP9~0rWcoOk}4;%l>SCZ-Y(-DUw&2b@eQgOHbxd4}*I4jV!Bg#U$op9X^`X zdcP~NGb64eAU4^>I`nB)72kUHapq>N10*QgIpG3m^dNK*?t3gF)RqxaGdMD)wM9UP zl8%q}7d5a|wH*T~F1z(jSLe=Z?bxE=u%S?Y9|6{m8G9fLnL0F!KdzwUfhX=HxqY`T4p4-}r zSUYqVSgQ&=g8E|Q=Oo!XYC^Z3hfglvJ2IyTKIU0fqL)~H*ku|17=Y(B*v~3HZNT^m zz6{2egU{+3v(Ep^$_{UUY2yM)uVJhR0L$4lfbJi zi=>Eb4_-Avd#)*gDS&pT;DQ5T%(^NqI(gZOLQ`q|0S(SQz|LUVaa{$UX~eN@{;=AZ zMJ&FVR%yS@+m|slq#n~^*Uc9ln)vF+{0gEq&WdV9V;3eXouCjGPY<-O=TZ%clLMCIIN9~d51vWt<|-|CH;DT6tz zzwMcb1pEjIsF$DIK%3}scaD?S;MsMS<@9~NrI4IF5aTu-Jd8p}?gqC+HORd#reK~E zgvn6X``gn16jo%*Y1=kQM??hbR{Xuo9P2iKT&;yrDz?xQ zmV8yQ%$~@myvXerw}ChwJPitL$n}1MRJP2fO*IuXH%RAs9W>S`s%|hwjvbW@`RqW&p-zy6h zs-=ju%10C_jdSfa}=v95)iVS?S2A@VCZ2#hyEpP_Yo!#R!lFST+N_dYsCuBL` zU2AZhJ+Uq=0wd(F%`Lr$*=l&o%=p=m4?fdS;uT)XI(6Ye#HuEi3bx!=7P7#r8$t5~ zZti3g;*H}+@{Y3vy!dK}uz;?VI;Rd1R=)TuL#0ILDDAm<`gL789@nzmYe4wBD3vb> zy;?t9*7&I|U(ttGjo3f6YRjbT!DGae1bD4C9lzqYpC@Lv#4CS0AMJ;H`*VObE|7_E z5VeGm==b|R)#qyhwMwv@fCvQmphnz7miJ$caeUi@R=Cc{z4n9!cqph zGf5;YS@)isZqa=_rpAkbQ@{=ACTnOj$y(K@@`r4Tq%DG=xg@bV6Q_GH!td83o&{f) zWm|(adJb8&Gu2fQ@XXsr491xcm}>Alnx!dmB?D}Zc7I;rg#*dH|N3{#6+ zyFx}UoyuE9tRsfJbW9`Mo^rKM1Y zB+%%Y-s>|MeC*;8pdGtWdJZQl6i4=dZoGjMXN(hyZ4sB(%5B^EzaJXs6!_ zve(e%j=e3Qcdi}YOggPR!dyw``wS4a4OzT=XuU53i**{+B>TX6fRam$ljNMt`zt!1 zNpT!9vKt=ukP>~YuJP4Cw3!RHnpI9T>-=e54Nc2fDlQyvc1(@1bM0bf?0*6+! zE`FZQ6rG>KIIcT7W!ai(n)N+C97LVbi?}NdU`jo`+FUuVlIcXxatZ(NhRouw z5{gEq3CnP@aH#A7%4|!pq%UpuA|68hG6Ue;Np8&H9kEfN*k!hBwfKiJbwvkA~WoCXL*T^ zVeW;2U-*`c;;d?iTOzJNOMX)?2aX%Y{W1 zVVHK2fIq`13osakL7gI%uoayt*sE55PuIfdMa%_-nnY+vegq^Q7796!&D}y+f^_hU zJTB5oX)c$oqxg4MPIfMMTm?q^)lFE>OI|)YGF^GIzrvM6ZKW>}038dFNnJm+T)AvL zCOrEQoRv;p%U4q+d~xmT?B<3jM>c{ z)i<9qTIAOTUX^$))+SlfsAoeWaCjfZ7`(#+jC<~3^y7iQ#*kUR$|gd2dsYwROUV?T0X|&VL=BP6lYtB z4A015&_{1vU+R~O>*CEue=?Np#C^0bD^$Q&5J3gxqcp_@NZI1xCcj_*KtqwBR%>pogI62Kv3#N1URHLYGErV*te? zor6}>$d0I)z#m-drB}n7E!cZ;y?g=HhQRd81`BqjB=2!B%rcVLSI+hIZA0ECStN(E zI3J~?xBe93eookhd?**)BF1;S$I(otkeV!^!Ow3be!RxwS8I@np%?)&gPbGk@V>V9_)N3p^5126rKEYHu@5MDdCv5bHeE}WO_y@L zV;YnN`O`kUbG*2A30F%D#Y! zaDqR;uJ}#q;=ypBh5$Wd?dWqLJ9%z8pmH&gb1=bNgZ1T(fc~ZdwRL6a_E<5ZlY9QS z;9K=`W2o5Nh;{2+tx8Gwx|wl~rov=h#6u-^=lgn+Cr?PQT>F*R{*C?(^8#9zc|X`K zGJnQ@e`Iu*InX8u%7B{9BONLhLG=`XNPc82bMbsW zfUTIs6^-u|eD!#T@EH7XvW6}Uo7nEc1L=)43a;yzr3_pq{2=srOpw`m^I(~UTBmCx zgsPmS=K~%QZI`7 z2FFP7aO`t`n7h$r7sAX0k~T#iq93CCCPd^FN!=QzpAed?faAF!gjT~14dA*S9h$is zUcb>Tq1V`2^K|@aexZBv@FH1xt4A1`%20lX6gUYL_`-EeoB`6Jv{~Lzc;*Dlj#hkq^NWcvupnaoySt%0<+{>Noi-7d?o=TduceiYGh(3fBqB7=%(x(@a7`f zJwrwjP-&3xl1f>VNx?|2c;j_XOsfqlJ|NhB0t2-C_$`^7rlZqMGNQ=8AC0$dR;ql& z&TY^=ssUn%015&{O<+dJXU}UyY+Q2KBSXCC?1Rt{j-3Knz1T`k zl=!J#5a+dA{vR-QIEZRJGYPzLD~vga7#aTysO;a^&~jjWb5ngCzDlghRPxsBP&$s` z?!T=f8<(S$i}gNnEmCTy8wfIJVqCKGQefXgo`dHK`?-8!ZoH7us&pbwitU8;uG8)F z0bKr`2SmZq5@`aqG#}4o2+eI+{0#8Keg|M5u1EN@fdkX`>$)akWBI)*vX0M2>SdLd zFo?stwU!#(;dG(=X*-mi2@ z-pt>03gj5G{J0_7tRFaTZ}T)%Ucr!(+YUC7%deQuKE#!WPOL`HRX&} zN2cIha>{v#kehsM3EV!37icNRqKsgxr2>jJ_?95_*YEtk*HBae|5ImP;P@Wc+y{Rn z9oI_kJnn#JNqKN*HiqYtHBK1L_w@mN3uY%gIE*=O(e1r5Xr+b6VYadsCjI~iJ;Llr zM88zx0i)qw6;$WqMvT|IS3j_%YY*XU8`0J{!GSJW-3g1EG$FC^%HGworL;Gy)kd4AWOgwaLkx03>~r|`ioUgiwG<&z0K#b@iDQZh8~%jy*~&67A!b&!0ig|YY6 z<4?(2KRMNyO&x5mo=qc5Uxkghvf2gJmT?0$qVn8*)l5RH@=HH%=VVhvw1r2VFsEO;1OS0+Sgesk$Y!}JA}Wb`mF?U~ zH-_K2ijss1#xj074y>%5L8kc)9PS11-r9eA+xIR5YWKjfy8b-h(1*C2)eMbTy{adw zOYo3V_oDk)qkBo2H+QYT3&t@8t*J+{=1aOmxGk4l)X!32n_+$i1Bt+Q5dH(ofwKHG}sU|t)W)wr91e7Z2+h;*V*Wt z^TbDtLX>Ehu|}s?%1)R?`d3UtN_Pmx6w}mN{~M0r?o8AX`!l7 zek7L)3QpHN@-g)j~Zt^9$u5EbCIDm%qnq}5uS&^p(G+9yVMm=56*HtUigXvjD(dT zWD#q$To0Zy!lGwtX z3@p3Qo&bwCEje}Hz25krKR`dZ2=tA>8EyX9S2Ehxh~C=%HTYtwz2E6rfO|hc&#a^B z|94&TKXtPIu1jWMp=Vlw|I_$(VV2iLB80wmQme99vk?tH253y$n?i;Q}hYznW-?0bN)l9=6vYly$6Q#F~ zElC5$8~2H+nM}sP^Rx2Lfr)yiYJ%L$#c--oPD;s*%!d?htyX(xn4Ja2o0L(Mg{(0A&+8u#CLJgCw86wAFO21ty+0+@W3eN|FAuJMNloy>Hy`7}%O^P0 zLnL=DIE1MLx$S42*ZS`koXlAZrCcX*jIwt(@)^AD`L5a6GB>@d)M2|kw|{kQ7o`8A z+>j$UNV~SZKhTJ!N65RB9gyq@ft`5O6=i~eCBAcI6JRT^gO`xaa+Bbi84kY3lYi5S zyU(xeTVX+s4`%YW*seky@Q4arm&cQ+(2!%eoVmu5jYWbeqi%>?TVx!6JwhdKV8Oc=q6rir_`4Afkx> zfIm*Pw;t+65b@B9?@cz{?CwDmmSnztZ{C|XZ*u#-GcUDso8e2bnlP|)p&c`Q>A3LWW zI^zc&O(ycw{w@Cf>iuMAqO#}gy|aHEy?#}E-u$wE_v!A}52HUfVjeg~X9YNJx9SPM z)i+s@UgsvUQg<9wvjJueOmUAOmpsocM=t&7n$`R($+}cLdaOknvf{>`qU8T;@gj<) z({dxCaW&VX+Z!{U0)v$$s`lyOn-4&2Lj*pVSrtJ!h(!jdiB`piY{kIY=LVkF{H1(fo0-cRz17MfUVb%cF6e=CD%ApL$?!%XP^*Msa`}B8?QK*?h*eFmi49dzbUD^P zNKh;xCqe-72>ni3j>3(k0s1IY4Oc`VVMS&X;aE+zr0mYlfRF>Ae&lvrq7fIv$e*58 zksMG@ks=J{Py}LXZkAJKQv(`W=giaIE5w~z*tE$hhqR{56567n}Qm}kgv_5K?gC@ykqh5~NGac}h}$04 p3MC@yVF;U^%1_{UA!x$h;>N31ilrM#Rb6IUHpu8)n6F(K`wLwzyFdT{ literal 0 HcmV?d00001 diff --git a/msg/CustomOdometry.msg b/msg/CustomOdometry.msg new file mode 100644 index 0000000..88118ce --- /dev/null +++ b/msg/CustomOdometry.msg @@ -0,0 +1,2 @@ +std_msgs/String odometryMode +nav_msgs/Odometry odometry \ No newline at end of file diff --git a/msg/OdometryStamped.msg b/msg/OdometryStamped.msg deleted file mode 100644 index f7abfc0..0000000 --- a/msg/OdometryStamped.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -float64 x -float64 y -float64 theta \ No newline at end of file diff --git a/src/odometry_node.cpp b/src/odometry_node.cpp index 0d6ae0e..f262e1e 100644 --- a/src/odometry_node.cpp +++ b/src/odometry_node.cpp @@ -1,5 +1,3 @@ -#include - #include "ros/ros.h" #include "ros/duration.h" @@ -16,14 +14,17 @@ #include #include -//#include "custom_messages/floatStamped.h" #include "odometry/floatStamped.h" +#include "odometry/CustomOdometry.h" + +// Numeric constants #define BASELINE 1.30 #define CAR_LENGTH 1.765 #define STEERING_FACTOR 18 #define PI 3.14159265 +// Default constants #define DEFAULT_INIT_POSITION_X 0.0 #define DEFAULT_INIT_POSITION_Y 0.0 #define DEFAULT_INIT_POSITION_THETA 0.0 @@ -34,18 +35,22 @@ #define DEFAULT_INIT_VELOCITY_RIGHT 0.0 #define DEFAULT_INIT_STEER 0.0 - +// Configuration constants #define DIFFERENTIAL_MODEL_MODE true -#define ACKERMAN_MODEL_MODE false +#define ACKERMANN_MODEL_MODE false +// Topic/Frame names definition #define SPEED_L_TOPIC "speedL_stamped" #define SPEED_R_TOPIC "speedR_stamped" #define STEER_TOPIC "steer_stamped" +#define CUSTOM_ODOMETRY_TOPIC_ID "custom_odometry" #define ODOMETRY_TOPIC_ID "odometry" #define ODOMETRY_FRAME_ID "odometry" #define CHILD_FRAME_ID "base_link" + +// Support data structures typedef struct last_odometry_data { double x; double y; @@ -76,7 +81,7 @@ void resetDefaultPosition() { } void configChangeCallback(odometry::parametersConfig& config, uint32_t level) { - ROS_INFO("Reconfigure Request:\nodometry_model_mode: %s\nodometry_reset_default: %s\nodometry_set_position: %s\nodometry_x_position: %f\nodometry_y_position: %f", + ROS_INFO("Reconfigure Request:\n\todometry_model_mode: %s\n\todometry_reset_default: %s\n\todometry_set_position: %s\n\todometry_x_position: %f\n\todometry_y_position: %f\n", config.odometry_model_mode?"True":"False", config.odometry_reset_default?"True":"False", config.odometry_set_position?"True":"False", @@ -92,54 +97,52 @@ void configChangeCallback(odometry::parametersConfig& config, uint32_t level) { resetDefaultPosition(); } -void differenrialDriveOdometry(double delta_time, double speed_L, double speed_R, OdometryData& new_odometry_data){ +void differenrialDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){ - double linear_velocity = (speed_R + speed_L) / 2; - double angular_velocity = (speed_R - speed_L) / BASELINE; - new_odometry_data.angular_velocity = angular_velocity; + double linear_velocity = (last_odometry_data.velocity_R + last_odometry_data.velocity_L) / 2; + double angular_velocity = last_odometry_data.angular_velocity; double delta_theta = angular_velocity * delta_time; new_odometry_data.theta = last_odometry_data.theta + delta_theta; - new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta); - new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta); - - + if (new_odometry_data.theta > 2*PI) { new_odometry_data.theta -= 2*PI; } else if (new_odometry_data.theta < 0) { new_odometry_data.theta += 2*PI; } - if (angular_velocity > 0.0001) //exact integration assuming velocities constamìnt in the time lapse - { - new_odometry_data.x = last_odometry_data.x + (linear_velocity / angular_velocity) * (sin(new_odometry_data.theta) - sin(last_odometry_data.theta)); - new_odometry_data.y = last_odometry_data.y - (linear_velocity / angular_velocity) * (cos(new_odometry_data.theta) - cos(last_odometry_data.theta)); + new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2); + new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2); + + + double new_linear_velocity = (speed_R + speed_L) / 2; + + new_odometry_data.angular_velocity = (speed_R - speed_L) / BASELINE; + + new_odometry_data.linear_velocity_x = new_linear_velocity * cos(new_odometry_data.theta); + new_odometry_data.linear_velocity_y = new_linear_velocity * sin(new_odometry_data.theta); + + new_odometry_data.steer = steer; + new_odometry_data.velocity_L = speed_L; + new_odometry_data.velocity_R = speed_R; + - } else { - new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2); - new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2); - } } -void ackermanDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){ - +void ackermannDriveOdometry(double delta_time, double speed_L, double speed_R, double steer, OdometryData& new_odometry_data){ + - double linear_velocity = (speed_R + speed_L) / 2; - double angular_velocity = linear_velocity * tan((steer / STEERING_FACTOR) * (PI / 180)) / CAR_LENGTH; - new_odometry_data.angular_velocity = angular_velocity; + double linear_velocity = (last_odometry_data.velocity_R + last_odometry_data.velocity_L) / 2; + double angular_velocity = last_odometry_data.angular_velocity; double delta_theta = angular_velocity * delta_time; - new_odometry_data.theta = last_odometry_data.theta + delta_theta; - - new_odometry_data.linear_velocity_x = linear_velocity * cos(new_odometry_data.theta); - new_odometry_data.linear_velocity_y = linear_velocity * sin(new_odometry_data.theta); if (new_odometry_data.theta > 2*PI) { @@ -147,20 +150,28 @@ void ackermanDriveOdometry(double delta_time, double speed_L, double speed_R, do } else if (new_odometry_data.theta < 0) { new_odometry_data.theta += 2*PI; } + + new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2); + new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2); + + double new_linear_velocity = (speed_R + speed_L) / 2; + + new_odometry_data.angular_velocity = new_linear_velocity * tan((steer / STEERING_FACTOR) * (PI / 180)) / CAR_LENGTH; + + new_odometry_data.linear_velocity_x = new_linear_velocity * cos(new_odometry_data.theta); + new_odometry_data.linear_velocity_y = new_linear_velocity * sin(new_odometry_data.theta); + + new_odometry_data.steer = steer; + new_odometry_data.velocity_L = speed_L; + new_odometry_data.velocity_R = speed_R; + - if (angular_velocity > 0.0001) //exact integration assuming velocities constamìnt in the time lapse - { - new_odometry_data.x = last_odometry_data.x + (linear_velocity / angular_velocity) * (sin(new_odometry_data.theta) - sin(last_odometry_data.theta)); - new_odometry_data.y = last_odometry_data.y - (linear_velocity / angular_velocity) * (cos(new_odometry_data.theta) - cos(last_odometry_data.theta)); - } else { - new_odometry_data.x = last_odometry_data.x + linear_velocity * delta_time * cos(last_odometry_data.theta + delta_theta / 2); - new_odometry_data.y = last_odometry_data.y + linear_velocity * delta_time * sin(last_odometry_data.theta + delta_theta / 2); - } } nav_msgs::Odometry populateOdometry() { + nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time::now(); odometry.header.frame_id = ODOMETRY_FRAME_ID; @@ -196,40 +207,53 @@ geometry_msgs::TransformStamped populateTransform() { return odometry_transform; } +odometry::CustomOdometry populateCustomOdometry(nav_msgs::Odometry odometry, string odometryModel) { + + odometry::CustomOdometry customOdometry; + customOdometry.odometry = odometry; + + customOdometry.odometryMode.data = odometryModel; + + return customOdometry; + +} + void subCallback(const odometry::floatStamped::ConstPtr& left, const odometry::floatStamped::ConstPtr& right, const odometry::floatStamped::ConstPtr& steer, tf::TransformBroadcaster& broadcaster, - ros::Publisher& publisher){ + ros::Publisher& publisherOdometry, + ros::Publisher& publisherCustomOdometry){ + string odometryModel; double time = (left->header.stamp.toSec() + right->header.stamp.toSec() + steer->header.stamp.toSec()) / 3; double delta_time = time - last_msg_time; last_msg_time = time; OdometryData new_odometry_data; if (last_config.odometry_model_mode == DIFFERENTIAL_MODEL_MODE) { - differenrialDriveOdometry(delta_time, left->data, right->data, new_odometry_data); - } else if (last_config.odometry_model_mode == ACKERMAN_MODEL_MODE) { - ackermanDriveOdometry(delta_time, left->data, right->data, steer->data, new_odometry_data); + differenrialDriveOdometry(delta_time, left->data, right->data, steer->data, new_odometry_data); + odometryModel = "Differential Drive"; + } else if (last_config.odometry_model_mode == ACKERMANN_MODEL_MODE) { + ackermannDriveOdometry(delta_time, left->data, right->data, steer->data, new_odometry_data); + odometryModel = "Ackermann Drive"; } else { - ROS_INFO("!!!!!!!!!!! CRITICAL CONFIG ERROR - ODOMETRY MODEL NOT SET!!!!!!!!!!!"); + ROS_INFO("!!!!!!!!!!! CRITICAL CONFIG ERROR - ODOMETRY MODEL NOT SET!!!!!!!!!!!\n\n"); } - - - double deltax = new_odometry_data.x - last_odometry_data.x; - double deltay = new_odometry_data.y - last_odometry_data.y; - double speed = (right->data + left->data) / 2; last_odometry_data = new_odometry_data; - ROS_INFO("[\nX COORDINATE: %f\nY COORDINATE: %f\nORIENTATION: %f\ndelta_time: %f\nSPEED: %f\nDELTA: %f %f", last_odometry_data.x, last_odometry_data.y, last_odometry_data.theta * 360/ (2*PI), delta_time, speed, deltax, deltay); // publishing and broadcasting nav_msgs::Odometry odometry = populateOdometry(); - publisher.publish(odometry); + publisherOdometry.publish(odometry); geometry_msgs::TransformStamped transform = populateTransform(); broadcaster.sendTransform(transform); + + odometry::CustomOdometry customOdometry = populateCustomOdometry(odometry, odometryModel); + publisherCustomOdometry.publish(customOdometry); + } @@ -249,6 +273,7 @@ int main(int argc, char *argv[]) last_odometry_data.velocity_R = DEFAULT_INIT_VELOCITY_RIGHT; last_odometry_data.steer = DEFAULT_INIT_STEER; last_msg_time = ros::Time::now().toSec(); + ROS_INFO("SUPPORT DATA STRUCTURES INITIALIZED\n"); // Config server init dynamic_reconfigure::Server config_server; @@ -256,23 +281,26 @@ int main(int argc, char *argv[]) // ### BEGIN ### output data publisher and broadcaster tf::TransformBroadcaster odometry_broadcaster; - ros::Publisher odometry_publisher = nh.advertise("odometry", 50); + ros::Publisher odometry_publisher = nh.advertise(ODOMETRY_TOPIC_ID, 50); + ros::Publisher custom_odometry_publisher = nh.advertise(CUSTOM_ODOMETRY_TOPIC_ID ,50); + + ROS_INFO("NODE STARTED\n"); // ### BEGIN ### wheels data subscriber and synchronizer - std::cout << "Node started...\n"; message_filters::Subscriber left_speed_sub(nh, SPEED_L_TOPIC, 1); message_filters::Subscriber right_speed_sub(nh, SPEED_R_TOPIC, 1); message_filters::Subscriber steer_sub(nh, STEER_TOPIC, 1); typedef message_filters::sync_policies::ApproximateTime syncPolicy; - std::cout << "Subscribers built...\n"; + ROS_INFO("SUBSCRIBER BUILT\n"); message_filters::Synchronizer sync(syncPolicy(10), left_speed_sub, right_speed_sub, steer_sub); - std::cout << "Synchronizer started...\n"; - sync.registerCallback(boost::bind(&subCallback, _1, _2, _3, odometry_broadcaster, odometry_publisher)); - std::cout << "Topics synchronized...\n"; + ROS_INFO("SYNCHRONIZER STARTED\n"); + sync.registerCallback(boost::bind(&subCallback, _1, _2, _3, odometry_broadcaster, odometry_publisher, custom_odometry_publisher)); + ROS_INFO("TOPICS SYNCHRONIZED\n"); // ### END ### + ROS_INFO("NODE RUNNING.....\n"); ros::spin(); return 0; } \ No newline at end of file