From 7719453071a28a3f42b63cac24c9515710f23781 Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Thu, 4 Dec 2025 15:17:02 -0800 Subject: [PATCH 01/13] added april tags, home world and fine tuned nav param for tight space navigation --- go2_gazebo_sim/go2_description/CMakeLists.txt | 15 + .../models/aruco_marker/model.config | 13 + .../models/aruco_marker/model.sdf | 32 + .../go2_description/tags/apriltags.png | Bin 0 -> 7570 bytes .../go2_description/worlds/home_world.sdf | 2316 +++++++++++++++++ .../go2_description/worlds/maze_world.sdf | 2 +- .../go2_gazebo_sim/launch/go2_launch.py | 48 +- go2_sdk/config/nav2_params.yaml | 51 +- 8 files changed, 2469 insertions(+), 8 deletions(-) create mode 100644 go2_gazebo_sim/go2_description/models/aruco_marker/model.config create mode 100644 go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf create mode 100644 go2_gazebo_sim/go2_description/tags/apriltags.png create mode 100644 go2_gazebo_sim/go2_description/worlds/home_world.sdf diff --git a/go2_gazebo_sim/go2_description/CMakeLists.txt b/go2_gazebo_sim/go2_description/CMakeLists.txt index 95b973b..93316cf 100644 --- a/go2_gazebo_sim/go2_description/CMakeLists.txt +++ b/go2_gazebo_sim/go2_description/CMakeLists.txt @@ -26,6 +26,21 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY + models + DESTINATION + share/${PROJECT_NAME}) + +install(DIRECTORY + models + DESTINATION + share/${PROJECT_NAME}) + +install(DIRECTORY + tags + DESTINATION + share/${PROJECT_NAME}) + ament_package() diff --git a/go2_gazebo_sim/go2_description/models/aruco_marker/model.config b/go2_gazebo_sim/go2_description/models/aruco_marker/model.config new file mode 100644 index 0000000..117af98 --- /dev/null +++ b/go2_gazebo_sim/go2_description/models/aruco_marker/model.config @@ -0,0 +1,13 @@ + + + aruco_marker + 1.0 + model.sdf + + OpenMind + hello@openmind.com + + + An ArUco marker for auto docking. + + diff --git a/go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf b/go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf new file mode 100644 index 0000000..9a9f2b9 --- /dev/null +++ b/go2_gazebo_sim/go2_description/models/aruco_marker/model.sdf @@ -0,0 +1,32 @@ + + + + true + + + + + 0.2 0.01 0.2 + + + + + + + 0.2 0.01 0.2 + + + + 1 1 1 1 + 1 1 1 1 + 0 0 0 1 + + + ../../tags/apriltags.png + + + + + + + diff --git a/go2_gazebo_sim/go2_description/tags/apriltags.png b/go2_gazebo_sim/go2_description/tags/apriltags.png new file mode 100644 index 0000000000000000000000000000000000000000..05f72358dde814fde5bbd26b5514effeb0b890d5 GIT binary patch literal 7570 zcmeHM2~<;ey1$Bws1%p67L;X*hzJM@f&u}nSOkPPf?C33aRd?+iLxXZ2w-WkE=XMn zZiEV^NZATuDaH_#5v?q;2M9?-L=7bOrukTqg=*N=STZc6w^BlKg` zkKPLXSViXNj#i&8SmAna$?`vGYF+gDYuWBE@u}agUsk5+)M62M=CfU1=a(3Nv|`Qj zw7)FZa;0w%IitnA>;7byWu`X&wmGY`;)Z{fJOaXJB;nfv(h~<>I2$DP<94MLtw@|*Zbs>Um-)##!moA60AfLG!!|y-;|L)&VlU`39 zf{C1h(bmz)q|fnsTcc!KHQjin`WUet?#TDa#}(0FcWYT+lb0lAbrt&gP!`xe93PMS#&-h)Up~(z3|7KutsX4Y!ZRPk z-#c=a!PxPfKH%c+zYqYYr4JT3cIFg;u!o^%WHr zTV$k!1gHH4ovFlW5jM%AN@GO`LW>x z^=UoBRaI4a7cN{#6{ep*f1AP{WV{^75hv$Q#!iJ<}?sK8Dl zAi%_c;dJ+i#9o-d?62}E4Qsj{)BpLZuxj=(UZw2CWo%e=Q~a6wb+~fEUn+UqQZ_NJ zSbpH+kY#f8e~`G-BGRk7V?SDKgc zv%iOyzFf>!^jcJ9Q-;i1yVp9?0flFYHeR+44mbd>?VfsidK)l@o_N!p{3vrnO!RIL z5)$so<{`;6--1pL?A6Ud1DkliZ(yL;k;Y?cwOUln8s31>_$rh-UT5g-q84FcLSI_9 zF8ovM)YS)_ zT(|TvR@OcbKqD~~363o2Ji;*D)ARW8ZjsiP889iBctr?HJs~c^qlXO)4ERYkkZ4{@ z%U+R|kDayk-kDO)7HT96%|?^eb`iT@Wsw7Eyb`toM`KMNDCpdW$Kz={7D?jaNFI(D zS9cZpx5GK>q*9i&#+DdJm!`{<$~-=cY(F`baWK&Rw2mPReRp@aZ`hpL^ES@TborEU zN>LRfdkv7Il*w!-NrHPPFH>CnlBUM8+g~=_VD<%8M}|PjIDw-Bqa)cNH~|;*TG<7L$;x z=26rXr7&??CCrN-t~PCuI7;u?@X11|Uu52uwsx#bmO;YEeTyoqdm|4DTjQFxaQ6(U zz3llNBfLC1#lzO_OnTnzSyfwCPwxpy)p;3ArE+GX41y!ODdQYw3`ALbq zN=vc&+7YRM%`mmr_~K)|eFdFmd;$O|;8`+ioWjBO)v$0(N}wFyl&FecI~w z3As2={xU{2{>X)%D9bg>pl`YbFuWC*z>CMec=2NRw3N$HvZV~K8+Kk|FXTGbcCPI+ zo^XmZ^yZBPCRgMXnCo{M(9`N3if%wdm> zXp<-p7U44FiQYl#p@Z#*(=i7q3#LsBvGe~0HXpZI@(UHTU%es-1hB&B(+KX zk1rZCOv?*XG<9h7THB?w)pfTNZ5l5xp`;9JWl?ps0*0D--{z^)<@c=uZw@L3(p~5V zR9UM<)tums)h{l2wv{Bx3nG-{A$>QGqLxeWn!<)4B`5g|IH- zrdcFMt%y-csxs*(a5_#S+b6oJYW4$zc#gg8Sl0`^TgiLsGMxZZjr{FvmFcr48J9Cv zA)6P5%vyjSvBCszJo#=gv;Ef2(`9{$18MeUeOcO-5j~e?R*jUDltg18)?6S<;Iqq; zsrxCKfPxemp-Yk(l=$v;4NW8#A03wzl+-w6xHoRfDLpfMZ>+g(#mn2j)Z2o%fizu~ zzFz-tQcu>NH=RzFL7DV1lyY>lE$TK*3&AXBSupL+3~LeM;-ICq^wG}#;gdgB^>zeO zwwrGQHU>3iiP5;8lbf3hCknf2MHP95%ZEA{`0YNv8#3Bf;8F<*nI4Qw?_1)0DGNY) z965S)tYUfNM?V)bllEzd_6NGU?paw`418l;L#7p0PGO_4_J;Y?`}BMIAS;r$)nb2w zyZ;V3pRD`(Z)a98Oc86hFa9GQ0Hwwpe7dy=OoVOH6u2Sp z0DISA82cy>KEq)?@R<;%>0o4g zMY>4K<^-pee{6sJ6UUY8_Z$Q;%=>{maM`BN70YG|u(Pq*KLFP=LA=y!?A22%%s@FE zN+)acv_k7UBg#Ru1X1e4q2vrYogU7v2D~jN3TwjI@3N!`c${=_WTZ|fFCFL`1->6y z9fU3+JcGgxw8G52$_k{)J7eUX2Qz`*fz&&Cdi>yD@)O3IL_PRyOE@h$_c4PR2Wj+8 z<~L8&>(1ES6S$O=6w%G-W-~YL*lkveHJnrpdV-&CRp!>(X_#R3Zr?X;<^Fl^)1v-*NkSA zK!dLv1Q=IVERU{F(Q*C+Z1>1p-#^?re-*Bf@D73(v3Ac{mi+a$lcOObEu?nsuX>Id ziOda*KjtuS-Ue(PtXO*mH;^}?($IYGHVBq2lT%=CL-h;{=ugVE0*>FGFFI>p{5T4< zTqIlGg^o(9fbMj1v!sWa*Utmc(iVD~F;EpO8h<;W&5?>k;p3J_V+cTy=yLXCNTtd% zbEB6h%_4l93fi%b=;-KZDz9jqQmfkGWm+)^LP48DG$}Y(;{EDov^R~1S|Cwgc5~DX zZY%-v)mKY_Df%h~@M70Nac7 z_4R?f?{so>=d~=F9kN8^6cpdx08MJaF-RQmZ%aDqqj+qb9-^Y=`)61 z6=)#RZ)Q*sc=cAa9m?mO{$o94#~&Na|lHoRj3&)FG7+a|~j(ED4n z#sYJwtD8k8!aLNndtlK*oS=OGW`V*vP!P(_ZU=vJS)#aT>{iB4?IEm7IOU9L$_Is| zfE-Xh+Z8nz=SDMk+e%fkgiYuuTv)U1l7ti~=9XY#t^6X1J!K{h0m#*(#os+Y zD|?*_HkgyBo+v*tP#>X;{^mdd#1@V`Npo|vMU{^ScybGX#n5UDk)EBjYr9pT;PKNlEBT&)dj)Qx*dnDvTxM@f;85Zo+pA-pkteIE z42hl8L7Wk0q*jB0Z&?fBOEjEdkA_S5FU5Jd?hE>k9$13=qYL`x zm_yB|hHrTQHZYK~!&6UR-}1+Ov&?I|T@MDt?uVSwFR>_)?I2b_is4(rWlr{fduV5u zWE*wthOkkg+E>E$@%Q%^aa<9?*>fB3t{1Cy5=T;cBCUdIoWxuW;q;V}N!7ll;zH0+ zDI{Nan_Z*+7&|}gJW^4pMB^*D^R!?DfxM7YHFE4$i!p2RFinH zTs@Jf1)lwQz*!-83l95EQpx+l$7N4XzH0BUJI(9^U^Oqfw^@Jdaia#wBf}&o_W1L2 zGtM6!TON$oD*Dp2i8bR zCXu{{)R#;=^%8jk0i8mjtgF7WC5BASTWNkEgf*Q;OfY6ojyCiRx39@rwtnb|dC49} z(wp^YkTp2@(saAAtMT5BPvND&)9Ggd9%4l1inDfX;lb+1;u;t%FxnT_D$~u6>4|^)XjtZIMI*F!Thv5$FV26t9+ZnSvxm z51uJFeQ9>w`AbmvJ0EzUH(zv{$+UqSuo9Jw@X!KkTf1beG@aVW^|03552MSL!E-MJ tclqa1bk@fEXA#41c!KnQ{bAdL;ppDcMF}exp-*2C_g&tu#XEvO{cldMYi9re literal 0 HcmV?d00001 diff --git a/go2_gazebo_sim/go2_description/worlds/home_world.sdf b/go2_gazebo_sim/go2_description/worlds/home_world.sdf new file mode 100644 index 0000000..88d4c24 --- /dev/null +++ b/go2_gazebo_sim/go2_description/worlds/home_world.sdf @@ -0,0 +1,2316 @@ + + + + + 0.001 + 1 + 1000 + + + + + + + ogre2 + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + true + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + + + + + + + 0 0 1 + 100 100 + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + + + + 0 0 0 0 0 0 + + + true + 0.5 5 1.25 0 0 0 + + + + + 11 0.15 2.5 + + + + + + + 11 0.15 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + + + + true + 0.5 -4 1.25 0 0 0 + + + + + 11 0.15 2.5 + + + + + + + 11 0.15 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + + + + true + -5 0.5 1.25 0 0 0 + + + + + 0.15 9 2.5 + + + + + + + 0.15 9 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + + + + true + 6 0.5 1.25 0 0 0 + + + + + 0.15 9 2.5 + + + + + + + 0.15 9 2.5 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + + + + true + -3.75 0.5 1.25 0 0 0 + + + + + 2.5 0.12 2.5 + + + + + + + 2.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -0.929556 0.5 2.3 0 0 0 + + + + + 1 0.12 0.4 + + + + + + + 1 0.12 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 0.318901 0.5 1.25 0 0 0 + + + + + 1.5 0.12 2.5 + + + + + + + 1.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 1.56303 0.5 2.3 0 0 0 + + + + + 1 0.12 0.4 + + + + + + + 1 0.12 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 2.31 0.5 1.25 0 0 0 + + + + + 0.5 0.12 2.5 + + + + + + + 0.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 4.25 0.5 1.25 0 0 0 + + + + + 3.5 0.12 2.5 + + + + + + + 3.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -2 0.205752 1.25 0 0 0 + + + + + 0.12 0.7 2.5 + + + + + + + 0.12 0.7 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -2 -0.634617 2.3 0 0 0 + + + + + 0.12 1 0.4 + + + + + + + 0.12 1 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -2 -2.75 1.25 0 0 0 + + + + + 0.12 2.5 2.5 + + + + + + + 0.12 2.5 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 2.5 3.25 1.25 0 0 0 + + + + + 0.12 3.5 2.5 + + + + + + + 0.12 3.5 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 2.5 0.959848 2.3 0 0 0 + + + + + 0.12 0.8 0.4 + + + + + + + 0.12 0.8 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + 2.5 -2.25 1.25 0 0 0 + + + + + 0.12 3.5 2.5 + + + + + + + 0.12 3.5 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -3.5 4 0.4 0 0 0 + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + -1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + + + true + -3.5 2.5 0.22 0 0 0 + + + + + 1.2 0.6 0.04 + + + + + + + 1.2 0.6 0.04 + + + + 0.45 0.35 0.25 1 + 0.55 0.45 0.35 1 + + + + -0.5 -0.2 -0.1 0 0 0 + + + 0.03 + 0.2 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.5 -0.2 -0.1 0 0 0 + + + 0.03 + 0.2 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + -0.5 0.2 -0.1 0 0 0 + + + 0.03 + 0.2 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.5 0.2 -0.1 0 0 0 + + + 0.03 + 0.2 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + + + true + -3.5 1.2 0.3 0 0 0 + + + + + 1.5 0.4 0.6 + + + + + + + 1.5 0.4 0.6 + + + + 0.2 0.15 0.1 1 + 0.3 0.25 0.2 1 + + + + + + true + -3.5 1.2 0.85 0 0 0 + + + + + 1.2 0.08 0.7 + + + + + + + 1.2 0.05 0.7 + + + + 0.05 0.05 0.05 1 + 0.1 0.1 0.1 1 + + + + 0 0.03 0 0 0 0 + + + 1.25 0.02 0.75 + + + + 0.15 0.15 0.15 1 + 0.2 0.2 0.2 1 + + + + + + true + -1.2 2.5 0.35 0 0 -1.57651 + + + + + 0.8 0.8 0.35 + + + + + + + 0.8 0.8 0.35 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + + 0 0.35 0.3 0 0 0 + + + 0.8 0.15 0.45 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + + + + true + -4.8 2.5 0.9 0 0 0 + + + + + 0.3 1 1.8 + + + + + + + 0.3 1 1.8 + + + + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + + + + 0 0 -0.6 0 0 0 + + + 0.28 0.95 0.02 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 0 0 0 0 0 + + + 0.28 0.95 0.02 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 0 0.6 0 0 0 + + + 0.28 0.95 0.02 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + + + true + -1.2 3.8034 0.3 0 0 0 + + + + + 0.25 + 0.6 + + + + + + + 0.25 + 0.6 + + + + 0.4 0.35 0.3 1 + 0.5 0.45 0.4 1 + + + + + + true + -1.2 3.8 0.6 0 0 0 + + + 0 0 0.05 0 0 0 + + + 0.1 + 0.1 + + + + 0.2 0.2 0.2 1 + 0.3 0.3 0.3 1 + + + + 0 0 0.5 0 0 0 + + + 0.02 + 0.8 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + 0 0 0.95 0 0 0 + + + 0.15 + 0.25 + + + + 0.9 0.85 0.7 1 + 0.95 0.9 0.8 1 + + + + + + true + 4 3.5 0 0 0 0 + + + 0 0 0.2 0 0 0 + + + 2.1 1.6 0.4 + + + + + 0 0 0.2 0 0 0 + + + 2.1 1.6 0.4 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 0 0.5 0 0 0 + + + 2 1.5 0.2 + + + + + 0 0 0.5 0 0 0 + + + 2 1.5 0.2 + + + + 0.9 0.9 0.85 1 + 0.95 0.95 0.9 1 + + + + 0 0.85 0.8 0 0 0 + + + 2.1 0.1 1 + + + + + 0 0.85 0.8 0 0 0 + + + 2.1 0.1 1 + + + + 0.3 0.2 0.12 1 + 0.4 0.3 0.2 1 + + + + -0.45 0.5 0.65 0 0 0 + + + 0.6 0.4 0.12 + + + + 0.85 0.85 0.9 1 + 0.9 0.9 0.95 1 + + + + 0.45 0.5 0.65 0 0 0 + + + 0.6 0.4 0.12 + + + + 0.85 0.85 0.9 1 + 0.9 0.9 0.95 1 + + + + + + true + 5.3 4.42414 0.3 0 0 0 + + + + + 0.5 0.4 0.6 + + + + + + + 0.5 0.4 0.6 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 -0.21 0 0 0 0 + + + 0.4 0.02 0.2 + + + + 0.3 0.2 0.1 1 + 0.4 0.3 0.2 1 + + + + + + true + 2.7 4.43203 0.3 0 0 0 + + + + + 0.5 0.4 0.6 + + + + + + + 0.5 0.4 0.6 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + + + true + 5.3 4.49184 0.6 0 0 0 + + + 0 0 0.05 0 0 0 + + + 0.08 + 0.1 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + 0 0 0.2 0 0 0 + + + 0.12 + 0.2 + + + + 0.95 0.9 0.8 1 + 1 0.95 0.85 1 + + + + + + true + 4 4.65 1 0 0 0 + + + + + 1.8 0.6 2 + + + + + + + 1.8 0.6 2 + + + + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + + + + -0.45 -0.31 0 0 0 0 + + + 0.85 0.02 1.9 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0.45 -0.31 0 0 0 0 + + + 0.85 0.02 1.9 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + + + true + 5.6997 1.19503 0.45 0 0 -1.54098 + + + + + 1.2 0.5 0.9 + + + + + + + 1.2 0.5 0.9 + + + + 0.4 0.3 0.2 1 + 0.5 0.4 0.3 1 + + + + 0 -0.26 0.3 0 0 0 + + + 1.1 0.02 0.25 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 -0.26 0 0 0 0 + + + 1.1 0.02 0.25 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + 0 -0.26 -0.3 0 0 0 + + + 1.1 0.02 0.25 + + + + 0.35 0.25 0.15 1 + 0.45 0.35 0.25 1 + + + + + + true + -1.94926 3.56402 0.35 0 0 -0.48062 + + + + + 0.5 0.5 0.35 + + + + + + + 0.5 0.5 0.08 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + 0 0.22 0.25 0 0 0 + + + 0.5 0.05 0.5 + + + + 0.6 0.55 0.5 1 + 0.7 0.65 0.6 1 + + + + -0.2 -0.2 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.2 -0.2 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + -0.2 0.2 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.2 0.2 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + + + true + -4.6 -1.75 0.45 0 0 0 + + + + + 0.65 4.5 0.9 + + + + + + + 0.65 4.5 0.9 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + 0 0 0.47 0 0 0 + + + 0.68 4.55 0.04 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + + + true + -4.6 -2.5 0.92 0 0 0 + + + + + 0.6 0.6 0.05 + + + + + + + 0.6 0.6 0.05 + + + + 0.1 0.1 0.1 1 + 0.15 0.15 0.15 1 + + + + -0.15 0.15 0.03 0 0 0 + + + 0.1 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + 0.15 0.15 0.03 0 0 0 + + + 0.08 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + -0.15 -0.15 0.03 0 0 0 + + + 0.08 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + 0.15 -0.15 0.03 0 0 0 + + + 0.1 + 0.02 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + + + true + -4.6 -0.5 0.9 0 0 0 + + + + + 0.5 0.6 0.2 + + + + + + + 0.5 0.6 0.2 + + + + 0.7 0.7 0.72 1 + 0.8 0.8 0.82 1 + + + + 0 0.2 0.2 0 0 0 + + + 0.02 + 0.3 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0 0.1 0.35 1.57 0 0 + + + 0.015 + 0.2 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + + + true + -4.7 -1.75 1.7 0 0 0 + + + + + 0.35 4 0.8 + + + + + + + 0.35 4 0.8 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + 0.18 -1.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + 0.18 -0.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + 0.18 0.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + 0.18 1.5 0 0 0 0 + + + 0.02 0.9 0.75 + + + + 0.8 0.8 0.75 1 + 0.85 0.85 0.8 1 + + + + + + true + -2.5 -3.5 0.9 0 0 0 + + + + + 0.8 0.7 1.8 + + + + + + + 0.8 0.7 1.8 + + + + 0.7 0.7 0.72 1 + 0.8 0.8 0.82 1 + + + + 0.41 -0.25 0.4 0 0 0 + + + 0.02 0.1 0.3 + + + + 0.5 0.5 0.52 1 + 0.6 0.6 0.62 1 + + + + 0.41 -0.25 -0.5 0 0 0 + + + 0.02 0.1 0.25 + + + + 0.5 0.5 0.52 1 + 0.6 0.6 0.62 1 + + + + + + true + -3.2 0 0.38 0 0 0 + + + + + 0.8 0.6 0.04 + + + + + + + 0.8 0.6 0.04 + + + + 0.45 0.35 0.25 1 + 0.55 0.45 0.35 1 + + + + -0.3 -0.2 -0.19 0 0 0 + + + 0.03 + 0.38 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.3 -0.2 -0.19 0 0 0 + + + 0.03 + 0.38 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + -0.3 0.2 -0.19 0 0 0 + + + 0.03 + 0.38 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + 0.3 0.2 -0.19 0 0 0 + + + 0.03 + 0.38 + + + + 0.3 0.25 0.2 1 + 0.4 0.35 0.3 1 + + + + + + true + -3.5 0 0.35 0 0 0 + + + + + 0.18 + 0.05 + + + + + + + 0.18 + 0.05 + + + + 0.55 0.45 0.35 1 + 0.65 0.55 0.45 1 + + + + 0.12 0 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + -0.06 0.1 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + -0.06 -0.1 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + + + true + -2.9 0 0.35 0 0 0 + + + + + 0.18 + 0.05 + + + + + + + 0.18 + 0.05 + + + + 0.55 0.45 0.35 1 + 0.65 0.55 0.45 1 + + + + 0.12 0 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + -0.06 0.1 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + -0.06 -0.1 -0.175 0 0 0 + + + 0.02 + 0.35 + + + + 0.3 0.3 0.3 1 + 0.4 0.4 0.4 1 + + + + + + true + -4.6 0.248494 1.05 0 0 0 + + + + + 0.35 0.5 0.3 + + + + + + + 0.35 0.5 0.3 + + + + 0.2 0.2 0.2 1 + 0.25 0.25 0.25 1 + + + + 0.18 0 0 0 0 0 + + + 0.02 0.4 0.25 + + + + 0.1 0.1 0.1 1 + 0.15 0.15 0.15 1 + + + + + + true + -2.2 -1.5 0.3 0 0 0 + + + + + 0.15 + 0.6 + + + + + + + 0.15 + 0.6 + + + + 0.4 0.4 0.42 1 + 0.5 0.5 0.52 1 + + + + + + true + 5 -3 0.3 0 0 0 + + + + + 1.7 0.8 0.6 + + + + + + + 1.7 0.8 0.6 + + + + 0.9 0.9 0.92 1 + 0.95 0.95 0.97 1 + + + + 0 0 0.05 0 0 0 + + + 1.5 0.6 0.5 + + + + 0.85 0.85 0.88 1 + 0.9 0.9 0.93 1 + + + + 0.7 0 0.35 0 0 0 + + + 0.03 + 0.15 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + + + true + 5 -3 2 0 0 0 + + + + + 1.8 0.02 0.02 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0 -0.4 -0.8 0 0 0 + + + 1.7 0.02 1.6 + + + + 0.8 0.85 0.9 1 + 0.85 0.9 0.95 1 + + + + + + true + 3.2 -3.5 0 0 0 1.57 + + + 0 0 0.2 0 0 0 + + + 0.65 0.4 0.4 + + + + + 0 0 0.2 0 0 0 + + + 0.65 0.4 0.4 + + + + 0.9 0.9 0.92 1 + 0.95 0.95 0.97 1 + + + + 0.15 0 0.35 0 0 0 + + + 0.18 + 0.1 + + + + 0.88 0.88 0.9 1 + 0.93 0.93 0.95 1 + + + + -0.2 0 0.55 0 0 0 + + + 0.2 0.38 0.35 + + + + + -0.2 0 0.55 0 0 0 + + + 0.2 0.38 0.35 + + + + 0.9 0.9 0.92 1 + 0.95 0.95 0.97 1 + + + + 0.1 0 0.45 0 0 0 + + + 0.45 0.38 0.05 + + + + 0.88 0.88 0.9 1 + 0.93 0.93 0.95 1 + + + + + + true + 3.2 -1.2 0.4 0 0 0 + + + + + 0.5 0.8 0.8 + + + + + + + 0.5 0.8 0.8 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + 0 0 0.42 0 0 0 + + + 0.55 0.85 0.04 + + + + 0.7 0.7 0.72 1 + 0.8 0.8 0.82 1 + + + + 0 0 0.45 0 0 0 + + + 0.2 + 0.15 + + + + 0.85 0.85 0.88 1 + 0.9 0.9 0.93 1 + + + + 0 0.25 0.6 0 0 0 + + + 0.02 + 0.2 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + + + true + 2.93 -1.2 1.4 0 0 0 + + + + + 0.05 0.7 0.9 + + + + 0.4 0.35 0.3 1 + 0.5 0.45 0.4 1 + + + + 0.03 0 0 0 0 0 + + + 0.01 0.6 0.8 + + + + 0.7 0.75 0.8 1 + 0.8 0.85 0.9 1 + + + + + + true + 5.8 -1.5 1 0 0 0 + + + + + 0.04 0.6 0.04 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0.15 0 -0.3 0 0 0 + + + 0.02 0.5 0.7 + + + + 0.3 0.5 0.6 1 + 0.4 0.6 0.7 1 + + + + + + true + 2.9 -2.8 0.7 0 0 0 + + + + + 0.05 0.15 0.1 + + + + 0.6 0.6 0.65 1 + 0.7 0.7 0.75 1 + + + + 0.08 0 0 1.57 0 0 + + + 0.06 + 0.12 + + + + 0.95 0.95 0.95 1 + 1 1 1 1 + + + + + + true + 3.6 -0.8 0.15 0 0 0 + + + + + 0.1 + 0.3 + + + + + + + 0.1 + 0.3 + + + + 0.8 0.8 0.82 1 + 0.85 0.85 0.87 1 + + + + + + true + -1.56216 0.500924 2.3 0 0 0 + + + + + 1 0.12 0.4 + + + + + + + 1 0.12 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -1.99415 -1.14358 2.3 0 0 0 + + + + + 0.12 1 0.4 + + + + + + + 0.12 1 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + true + -1.38418 -3.30512 0.35 0 0 1.5412 + + + + + 0.8 0.8 0.35 + + + + + + + 0.8 0.8 0.35 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + + 0 0.35 0.3 0 0 0 + + + 0.8 0.15 0.45 + + + + 0.5 0.4 0.3 1 + 0.6 0.5 0.4 1 + + + + + + true + 1.29202 -1.66458 0.4 0 0 0 + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + + 0 0 0 0 0 0 + + + 2.2 0.9 0.45 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + + 0 0.35 0.35 0 0 0 + + + 2.2 0.2 0.5 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + -1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + 1.0 0 0.15 0 0 0 + + + 0.2 0.9 0.3 + + + + 0.3 0.35 0.5 1 + 0.4 0.45 0.6 1 + + + + + + true + 2.50165 1.39644 2.3 0 0 0 + + + + + 0.12 0.8 0.4 + + + + + + + 0.12 0.8 0.4 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + + + + 0 0 10 0 0 0 + true + 1 + -0.5 0.1 -0.9 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.01 + 0.9 + 0.001 + + + + -2 2 2.4 0 0 0 + false + 1 + 0 0 -1 + 1 0.95 0.8 1 + 0.1 0.1 0.1 1 + + 10 + 0.1 + 0.5 + 0.01 + + + + 3.5 3 2.4 0 0 0 + false + 1 + 0 0 -1 + 1 0.95 0.8 1 + 0.1 0.1 0.1 1 + + 8 + 0.1 + 0.5 + 0.01 + + + + -2 -2.5 2.4 0 0 0 + false + 1 + 0 0 -1 + 1 1 1 1 + 0.2 0.2 0.2 1 + + 6 + 0.1 + 0.5 + 0.01 + + + + 3.5 -2 2.4 0 0 0 + false + 1 + 0 0 -1 + 1 1 1 1 + 0.2 0.2 0.2 1 + + 5 + 0.1 + 0.5 + 0.01 + + + + \ No newline at end of file diff --git a/go2_gazebo_sim/go2_description/worlds/maze_world.sdf b/go2_gazebo_sim/go2_description/worlds/maze_world.sdf index eb4ffdc..51f8cb6 100644 --- a/go2_gazebo_sim/go2_description/worlds/maze_world.sdf +++ b/go2_gazebo_sim/go2_description/worlds/maze_world.sdf @@ -670,4 +670,4 @@ - + \ No newline at end of file diff --git a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py index ca67202..79be63e 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py +++ b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py @@ -8,6 +8,7 @@ ExecuteProcess, IncludeLaunchDescription, TimerAction, + SetEnvironmentVariable, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -32,7 +33,25 @@ def generate_launch_description(): gait_config = os.path.join(go2_gazebo_sim, "config/gait/gait.yaml") links_config = os.path.join(go2_gazebo_sim, "config/links/links.yaml") default_model_path = os.path.join(go2_description, "urdf/unitree_go2_robot.xacro") - default_world_path = os.path.join(go2_description, "worlds/maze_world.sdf") + default_world_path = os.path.join(go2_description, "worlds/home_world.sdf") + + # Add go2_description/models to GZ_SIM_RESOURCE_PATH + go2_description_models = os.path.join(go2_description, "models") + + # Ensure GZ_SIM_RESOURCE_PATH includes the models directory + current_gz_resource_path = os.environ.get("GZ_SIM_RESOURCE_PATH", "") + if go2_description_models not in current_gz_resource_path: + if current_gz_resource_path: + new_gz_resource_path = current_gz_resource_path + ":" + go2_description_models + else: + new_gz_resource_path = go2_description_models + else: + new_gz_resource_path = current_gz_resource_path + + set_gz_resource_path = SetEnvironmentVariable( + name="GZ_SIM_RESOURCE_PATH", + value=new_gz_resource_path + ) declare_use_sim_time = DeclareLaunchArgument( "use_sim_time", @@ -247,6 +266,31 @@ def generate_launch_description(): }.items(), ) + # Spawn ArUco Marker + # We use the absolute path to the model.sdf to avoid model:// URI resolution issues + aruco_model_path = os.path.join(go2_description, "models/aruco_marker/model.sdf") + + gazebo_spawn_aruco = Node( + package="ros_gz_sim", + executable="create", + name="spawn_aruco", + output="screen", + arguments=[ + "-name", + "aruco_marker", + "-file", + aruco_model_path, + "-x", + "-1.91", + "-y", + "-1.99", + "-z", + "0.1", + "-Y", + "1.57", + ], + ) + # Spawn robot in Gazebo Sim gazebo_spawn_robot = Node( package="ros_gz_sim", @@ -410,6 +454,7 @@ def generate_launch_description(): return LaunchDescription( [ # Launch arguments + set_gz_resource_path, declare_use_sim_time, declare_rviz, declare_robot_name, @@ -427,6 +472,7 @@ def generate_launch_description(): gz_sim, robot_state_publisher_node, gazebo_spawn_robot, + gazebo_spawn_aruco, gazebo_bridge, # CHAMP controller nodes quadruped_controller_node, diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index 5f9fded..e20c9ac 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -243,7 +243,7 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - footprint: "[[0.3, 0.2], [0.3, -0.2], [-0.3, -0.2], [-0.3, 0.2]]" + footprint: "[[0.3, 0.2], [0.3, -0.45], [-0.3, -0.45], [-0.3, 0.2]]" plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] static_layer: @@ -309,7 +309,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 5.0 - inflation_radius: 0.25 + inflation_radius: 0.4 always_send_full_costmap: True global_costmap: @@ -347,7 +347,7 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 5.0 - inflation_radius: 0.30 + inflation_radius: 0.5 always_send_full_costmap: True @@ -370,16 +370,55 @@ planner_server: transform_tolerance: 0.1 use_sim_time: False planner_plugins: ["GridBased"] - # Using SmacPlanner2D for better orientation handling + # SmacPlannerHybrid with Reeds-Shepp for backward motion in narrow lanes GridBased: - plugin: "nav2_smac_planner/SmacPlanner2D" + plugin: "nav2_smac_planner/SmacPlannerHybrid" tolerance: 0.3 downsample_costmap: False downsampling_factor: 1 allow_unknown: True max_iterations: 1000000 max_on_approach_iterations: 1000 + max_planning_time: 5.0 smooth_path: True + + # Reeds-Shepp motion model - enables forward AND backward motion + motion_model_for_search: "REEDS_SHEPP" + + # Angular resolution (72 bins = 5 degree resolution) + angle_quantization_bins: 72 + + # Analytic expansion parameters + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + + # Minimum turning radius (meters) - tuned for Go2's turning capability + minimum_turning_radius: 0.1 + + # Reverse penalty - lower value encourages reverse when beneficial for narrow lanes + reverse_penalty: 0.1 + + # Change of direction penalty - low to allow direction switches in tight spaces + change_penalty: 5.15 + non_straight_penalty: 0.4 + + # Cost penalties + cost_penalty: 2.0 + retrospective_penalty: 0.015 + + # Lookup table size for hybrid search + lookup_table_size: 20.0 + + # Cache obstacle heuristic for performance + cache_obstacle_heuristic: False + + # Smoothing parameters + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: True smoother_server: ros__parameters: @@ -447,4 +486,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 + velocity_timeout: 1.0 \ No newline at end of file From 7848c60a253c0416e22d185bec0cfd3df5556a2d Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Wed, 10 Dec 2025 14:15:58 -0800 Subject: [PATCH 02/13] Add simulation navigation parameters and update home_world sdf and gait params --- .../go2_description/worlds/home_world.sdf | 3156 +++++++++++++++-- .../go2_gazebo_sim/config/gait/gait.yaml | 6 +- go2_sdk/config/nav2_params.yaml | 284 +- go2_sdk/config/nav2_params_sim.yaml | 453 +++ 4 files changed, 3395 insertions(+), 504 deletions(-) create mode 100644 go2_sdk/config/nav2_params_sim.yaml diff --git a/go2_gazebo_sim/go2_description/worlds/home_world.sdf b/go2_gazebo_sim/go2_description/worlds/home_world.sdf index 88d4c24..85a30af 100644 --- a/go2_gazebo_sim/go2_description/worlds/home_world.sdf +++ b/go2_gazebo_sim/go2_description/worlds/home_world.sdf @@ -1,4 +1,3 @@ - @@ -51,12 +50,27 @@ 0.3 0.3 0.3 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false - 0 0 0 0 0 0 + 0 0 0 0 -0 0 + false true - 0.5 5 1.25 0 0 0 + 0.5 5 1.25 0 -0 0 @@ -64,6 +78,13 @@ 11 0.15 2.5 + + + + + + + @@ -76,11 +97,26 @@ 0.95 0.95 0.9 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 0.5 -4 1.25 0 0 0 + 0.5 -4 1.25 0 -0 0 @@ -88,6 +124,13 @@ 11 0.15 2.5 + + + + + + + @@ -100,11 +143,26 @@ 0.95 0.95 0.9 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -5 0.5 1.25 0 0 0 + -5 0.5 1.25 0 -0 0 @@ -112,6 +170,13 @@ 0.15 9 2.5 + + + + + + + @@ -124,11 +189,26 @@ 0.95 0.95 0.9 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 6 0.5 1.25 0 0 0 + 6 0.5 1.25 0 -0 0 @@ -136,6 +216,13 @@ 0.15 9 2.5 + + + + + + + @@ -148,11 +235,26 @@ 0.95 0.95 0.9 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -3.75 0.5 1.25 0 0 0 + -3.75 0.5 1.25 0 -0 0 @@ -160,6 +262,13 @@ 2.5 0.12 2.5 + + + + + + + @@ -172,11 +281,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -0.929556 0.5 2.3 0 0 0 + -0.929556 0.5 2.3 0 -0 0 @@ -184,6 +308,13 @@ 1 0.12 0.4 + + + + + + + @@ -196,11 +327,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 0.318901 0.5 1.25 0 0 0 + 0.318901 0.5 1.25 0 -0 0 @@ -208,6 +354,13 @@ 1.5 0.12 2.5 + + + + + + + @@ -220,11 +373,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 1.56303 0.5 2.3 0 0 0 + 1.56303 0.5 2.3 0 -0 0 @@ -232,6 +400,13 @@ 1 0.12 0.4 + + + + + + + @@ -244,11 +419,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.31 0.5 1.25 0 0 0 + 2.31 0.5 1.25 0 -0 0 @@ -256,6 +446,13 @@ 0.5 0.12 2.5 + + + + + + + @@ -268,11 +465,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 4.25 0.5 1.25 0 0 0 + 4.25 0.5 1.25 0 -0 0 @@ -280,6 +492,13 @@ 3.5 0.12 2.5 + + + + + + + @@ -292,11 +511,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -2 0.205752 1.25 0 0 0 + -2 0.205752 1.25 0 -0 0 @@ -304,6 +538,13 @@ 0.12 0.7 2.5 + + + + + + + @@ -316,11 +557,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -2 -0.634617 2.3 0 0 0 + -2 -0.634617 2.3 0 -0 0 @@ -328,6 +584,13 @@ 0.12 1 0.4 + + + + + + + @@ -340,11 +603,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -2 -2.75 1.25 0 0 0 + -2 -2.75 1.25 0 -0 0 @@ -352,6 +630,13 @@ 0.12 2.5 2.5 + + + + + + + @@ -364,11 +649,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.5 3.25 1.25 0 0 0 + 2.5 3.25 1.25 0 -0 0 @@ -376,6 +676,13 @@ 0.12 3.5 2.5 + + + + + + + @@ -388,11 +695,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.5 0.959848 2.3 0 0 0 + 2.5 0.959848 2.3 0 -0 0 @@ -400,6 +722,13 @@ 0.12 0.8 0.4 + + + + + + + @@ -412,11 +741,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.5 -2.25 1.25 0 0 0 + 2.5 -2.25 1.25 0 -0 0 @@ -424,6 +768,13 @@ 0.12 3.5 2.5 + + + + + + + @@ -436,11 +787,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -3.5 4 0.4 0 0 0 + -3.8127 4.56578 0.4 0 -0 0 0 0 0 0 0 0 @@ -449,6 +815,13 @@ 2.2 0.9 0.45 + + + + + + + 0 0 0 0 0 0 @@ -469,6 +842,13 @@ 2.2 0.2 0.5 + + + + + + + 0 0.35 0.35 0 0 0 @@ -506,11 +886,26 @@ 0.4 0.45 0.6 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -3.5 2.5 0.22 0 0 0 + -3.86253 3.48221 0.22 0 -0 0 @@ -518,6 +913,13 @@ 1.2 0.6 0.04 + + + + + + + @@ -534,8 +936,8 @@ -0.5 -0.2 -0.1 0 0 0 - 0.03 - 0.2 + 0.029999999999999999 + 0.20000000000000001 @@ -547,8 +949,8 @@ 0.5 -0.2 -0.1 0 0 0 - 0.03 - 0.2 + 0.029999999999999999 + 0.20000000000000001 @@ -560,8 +962,8 @@ -0.5 0.2 -0.1 0 0 0 - 0.03 - 0.2 + 0.029999999999999999 + 0.20000000000000001 @@ -573,8 +975,8 @@ 0.5 0.2 -0.1 0 0 0 - 0.03 - 0.2 + 0.029999999999999999 + 0.20000000000000001 @@ -582,11 +984,26 @@ 0.4 0.35 0.3 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -3.5 1.2 0.3 0 0 0 + -3.5 0.784214 0.3 0 -0 0 @@ -594,6 +1011,13 @@ 1.5 0.4 0.6 + + + + + + + @@ -606,11 +1030,26 @@ 0.3 0.25 0.2 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -3.5 1.2 0.85 0 0 0 + -3.5 0.682625 0.85 0 -0 0 @@ -618,6 +1057,13 @@ 1.2 0.08 0.7 + + + + + + + @@ -642,11 +1088,26 @@ 0.2 0.2 0.2 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -1.2 2.5 0.35 0 0 -1.57651 + -1.66029 2.53221 0.35 0 0 -1.57651 @@ -654,6 +1115,13 @@ 0.8 0.8 0.35 + + + + + + + @@ -678,11 +1146,26 @@ 0.6 0.5 0.4 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -4.8 2.5 0.9 0 0 0 + -4.8 2.5 0.9 0 -0 0 @@ -690,6 +1173,13 @@ 0.3 1 1.8 + + + + + + + @@ -738,25 +1228,47 @@ 0.45 0.35 0.25 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -1.2 3.8034 0.3 0 0 0 + -1.2 4.65187 0.3 0 -0 0 0.25 - 0.6 + 0.59999999999999998 + + + + + + + 0.25 - 0.6 + 0.59999999999999998 @@ -764,18 +1276,33 @@ 0.5 0.45 0.4 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -1.2 3.8 0.6 0 0 0 + -1.2 4.63964 0.6 0 -0 0 0 0 0.05 0 0 0 - 0.1 - 0.1 + 0.10000000000000001 + 0.10000000000000001 @@ -788,7 +1315,7 @@ 0.02 - 0.8 + 0.80000000000000004 @@ -800,7 +1327,7 @@ 0 0 0.95 0 0 0 - 0.15 + 0.14999999999999999 0.25 @@ -809,11 +1336,26 @@ 0.95 0.9 0.8 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 4 3.5 0 0 0 0 + 4 3.5 0 0 -0 0 0 0 0.2 0 0 0 @@ -822,6 +1364,13 @@ 2.1 1.6 0.4 + + + + + + + 0 0 0.2 0 0 0 @@ -842,6 +1391,13 @@ 2 1.5 0.2 + + + + + + + 0 0 0.5 0 0 0 @@ -862,6 +1418,13 @@ 2.1 0.1 1 + + + + + + + 0 0.85 0.8 0 0 0 @@ -899,11 +1462,26 @@ 0.9 0.9 0.95 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 5.3 4.42414 0.3 0 0 0 + 5.3 4.42414 0.3 0 -0 0 @@ -911,6 +1489,13 @@ 0.5 0.4 0.6 + + + + + + + @@ -935,11 +1520,26 @@ 0.4 0.3 0.2 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.7 4.43203 0.3 0 0 0 + 2.7 2.89912 0.3 0 -0 0 @@ -947,6 +1547,13 @@ 0.5 0.4 0.6 + + + + + + + @@ -959,18 +1566,33 @@ 0.45 0.35 0.25 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 5.3 4.49184 0.6 0 0 0 + 5.3 4.49184 0.6 0 -0 0 0 0 0.05 0 0 0 - 0.08 - 0.1 + 0.080000000000000002 + 0.10000000000000001 @@ -983,7 +1605,7 @@ 0.12 - 0.2 + 0.20000000000000001 @@ -991,11 +1613,26 @@ 1 0.95 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 4 4.65 1 0 0 0 + 4 4.65 1 0 -0 0 @@ -1003,6 +1640,13 @@ 1.8 0.6 2 + + + + + + + @@ -1039,11 +1683,26 @@ 0.45 0.35 0.25 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 5.6997 1.19503 0.45 0 0 -1.54098 + 5.50802 3.38724 0.45 0 0 -1.54098 @@ -1051,6 +1710,13 @@ 1.2 0.5 0.9 + + + + + + + @@ -1099,11 +1765,26 @@ 0.45 0.35 0.25 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -1.94926 3.56402 0.35 0 0 -0.48062 + -1.71989 3.44443 0.35 0 0 -0.48062 @@ -1111,6 +1792,13 @@ 0.5 0.5 0.35 + + + + + + + @@ -1140,7 +1828,7 @@ 0.02 - 0.35 + 0.34999999999999998 @@ -1153,7 +1841,7 @@ 0.02 - 0.35 + 0.34999999999999998 @@ -1166,7 +1854,7 @@ 0.02 - 0.35 + 0.34999999999999998 @@ -1179,7 +1867,7 @@ 0.02 - 0.35 + 0.34999999999999998 @@ -1187,11 +1875,26 @@ 0.4 0.35 0.3 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -4.6 -1.75 0.45 0 0 0 + -4.6 -1.75 0.45 0 -0 0 @@ -1199,6 +1902,13 @@ 0.65 4.5 0.9 + + + + + + + @@ -1223,11 +1933,26 @@ 0.4 0.4 0.4 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -4.6 -2.5 0.92 0 0 0 + -4.6 -2.5 0.92 0 -0 0 @@ -1235,6 +1960,13 @@ 0.6 0.6 0.05 + + + + + + + @@ -1251,7 +1983,7 @@ -0.15 0.15 0.03 0 0 0 - 0.1 + 0.10000000000000001 0.02 @@ -1264,7 +1996,7 @@ 0.15 0.15 0.03 0 0 0 - 0.08 + 0.080000000000000002 0.02 @@ -1277,7 +2009,7 @@ -0.15 -0.15 0.03 0 0 0 - 0.08 + 0.080000000000000002 0.02 @@ -1290,7 +2022,7 @@ 0.15 -0.15 0.03 0 0 0 - 0.1 + 0.10000000000000001 0.02 @@ -1299,11 +2031,26 @@ 0.25 0.25 0.25 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -4.6 -0.5 0.9 0 0 0 + -4.6 -0.5 0.9 0 -0 0 @@ -1311,6 +2058,13 @@ 0.5 0.6 0.2 + + + + + + + @@ -1328,7 +2082,7 @@ 0.02 - 0.3 + 0.29999999999999999 @@ -1340,8 +2094,8 @@ 0 0.1 0.35 1.57 0 0 - 0.015 - 0.2 + 0.014999999999999999 + 0.20000000000000001 @@ -1349,11 +2103,26 @@ 0.7 0.7 0.75 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -4.7 -1.75 1.7 0 0 0 + -4.7 -1.75 1.7 0 -0 0 @@ -1361,6 +2130,13 @@ 0.35 4 0.8 + + + + + + + @@ -1421,11 +2197,26 @@ 0.85 0.85 0.8 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -2.5 -3.5 0.9 0 0 0 + -2.5 -3.5 0.9 0 -0 0 @@ -1433,10 +2224,17 @@ 0.8 0.7 1.8 - - - - + + + + + + + + + + + 0.8 0.7 1.8 @@ -1469,217 +2267,26 @@ 0.6 0.6 0.62 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false - - - true - -3.2 0 0.38 0 0 0 - - - - - 0.8 0.6 0.04 - - - - - - - 0.8 0.6 0.04 - - - - 0.45 0.35 0.25 1 - 0.55 0.45 0.35 1 - - - - -0.3 -0.2 -0.19 0 0 0 - - - 0.03 - 0.38 - - - - 0.3 0.25 0.2 1 - 0.4 0.35 0.3 1 - - - - 0.3 -0.2 -0.19 0 0 0 - - - 0.03 - 0.38 - - - - 0.3 0.25 0.2 1 - 0.4 0.35 0.3 1 - - - - -0.3 0.2 -0.19 0 0 0 - - - 0.03 - 0.38 - - - - 0.3 0.25 0.2 1 - 0.4 0.35 0.3 1 - - - - 0.3 0.2 -0.19 0 0 0 - - - 0.03 - 0.38 - - - - 0.3 0.25 0.2 1 - 0.4 0.35 0.3 1 - - - - - - true - -3.5 0 0.35 0 0 0 - - - - - 0.18 - 0.05 - - - - - - - 0.18 - 0.05 - - - - 0.55 0.45 0.35 1 - 0.65 0.55 0.45 1 - - - - 0.12 0 -0.175 0 0 0 - - - 0.02 - 0.35 - - - - 0.3 0.3 0.3 1 - 0.4 0.4 0.4 1 - - - - -0.06 0.1 -0.175 0 0 0 - - - 0.02 - 0.35 - - - - 0.3 0.3 0.3 1 - 0.4 0.4 0.4 1 - - - - -0.06 -0.1 -0.175 0 0 0 - - - 0.02 - 0.35 - - - - 0.3 0.3 0.3 1 - 0.4 0.4 0.4 1 - - - - - - true - -2.9 0 0.35 0 0 0 - - - - - 0.18 - 0.05 - - - - - - - 0.18 - 0.05 - - - - 0.55 0.45 0.35 1 - 0.65 0.55 0.45 1 - - - - 0.12 0 -0.175 0 0 0 - - - 0.02 - 0.35 - - - - 0.3 0.3 0.3 1 - 0.4 0.4 0.4 1 - - - - -0.06 0.1 -0.175 0 0 0 - - - 0.02 - 0.35 - - - - 0.3 0.3 0.3 1 - 0.4 0.4 0.4 1 - - - - -0.06 -0.1 -0.175 0 0 0 - - - 0.02 - 0.35 - - - - 0.3 0.3 0.3 1 - 0.4 0.4 0.4 1 - - - + false true - -4.6 0.248494 1.05 0 0 0 + -4.6 0.248494 1.05 0 -0 0 @@ -1687,6 +2294,13 @@ 0.35 0.5 0.3 + + + + + + + @@ -1711,25 +2325,47 @@ 0.15 0.15 0.15 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -2.2 -1.5 0.3 0 0 0 + -2.2 -1.5 0.3 0 -0 0 - 0.15 - 0.6 + 0.14999999999999999 + 0.59999999999999998 + + + + + + + - 0.15 - 0.6 + 0.14999999999999999 + 0.59999999999999998 @@ -1737,11 +2373,26 @@ 0.5 0.5 0.52 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 5 -3 0.3 0 0 0 + 5 -3.46048 0.3 0 -0 0 @@ -1749,6 +2400,13 @@ 1.7 0.8 0.6 + + + + + + + @@ -1777,8 +2435,8 @@ 0.7 0 0.35 0 0 0 - 0.03 - 0.15 + 0.029999999999999999 + 0.14999999999999999 @@ -1786,11 +2444,26 @@ 0.7 0.7 0.75 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 5 -3 2 0 0 0 + 5 -3.4766 2 0 -0 0 @@ -1815,11 +2488,26 @@ 0.85 0.9 0.95 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 3.2 -3.5 0 0 0 1.57 + 2.80821 -3.49969 0 0 -0 1.57 0 0 0.2 0 0 0 @@ -1828,6 +2516,13 @@ 0.65 0.4 0.4 + + + + + + + 0 0 0.2 0 0 0 @@ -1845,8 +2540,8 @@ 0.15 0 0.35 0 0 0 - 0.18 - 0.1 + 0.17999999999999999 + 0.10000000000000001 @@ -1861,6 +2556,13 @@ 0.2 0.38 0.35 + + + + + + + -0.2 0 0.55 0 0 0 @@ -1886,11 +2588,26 @@ 0.93 0.93 0.95 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 3.2 -1.2 0.4 0 0 0 + 2.84233 -1.2 0.4 0 -0 0 @@ -1898,6 +2615,13 @@ 0.5 0.8 0.8 + + + + + + + @@ -1926,8 +2650,8 @@ 0 0 0.45 0 0 0 - 0.2 - 0.15 + 0.20000000000000001 + 0.14999999999999999 @@ -1940,7 +2664,7 @@ 0.02 - 0.2 + 0.20000000000000001 @@ -1948,11 +2672,26 @@ 0.7 0.7 0.75 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.93 -1.2 1.4 0 0 0 + 2.59974 -1.2 1.4 0 -0 0 @@ -1977,11 +2716,26 @@ 0.8 0.85 0.9 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 5.8 -1.5 1 0 0 0 + 5.8 -1.5 1 0 -0 0 @@ -2006,11 +2760,26 @@ 0.4 0.6 0.7 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.9 -2.8 0.7 0 0 0 + 2.59852 -2.8 0.7 0 -0 0 @@ -2027,7 +2796,7 @@ 0.08 0 0 1.57 0 0 - 0.06 + 0.059999999999999998 0.12 @@ -2036,37 +2805,26 @@ 1 1 1 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false - - - true - 3.6 -0.8 0.15 0 0 0 - - - - - 0.1 - 0.3 - - - - - - - 0.1 - 0.3 - - - - 0.8 0.8 0.82 1 - 0.85 0.85 0.87 1 - - - + false true - -1.56216 0.500924 2.3 0 0 0 + -1.56216 0.500924 2.3 0 -0 0 @@ -2074,6 +2832,13 @@ 1 0.12 0.4 + + + + + + + @@ -2086,11 +2851,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -1.99415 -1.14358 2.3 0 0 0 + -1.99415 -1.14358 2.3 0 -0 0 @@ -2098,6 +2878,13 @@ 0.12 1 0.4 + + + + + + + @@ -2110,11 +2897,26 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - -1.38418 -3.30512 0.35 0 0 1.5412 + -1.52043 -3.40428 0.35 0 -0 1.5412 @@ -2122,6 +2924,13 @@ 0.8 0.8 0.35 + + + + + + + @@ -2146,11 +2955,26 @@ 0.6 0.5 0.4 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 1.29202 -1.66458 0.4 0 0 0 + 1.29202 -1.66458 0.4 0 -0 0 0 0 0 0 0 0 @@ -2159,8 +2983,15 @@ 2.2 0.9 0.45 - - + + + + + + + + + 0 0 0 0 0 0 @@ -2179,6 +3010,13 @@ 2.2 0.2 0.5 + + + + + + + 0 0.35 0.35 0 0 0 @@ -2216,11 +3054,26 @@ 0.4 0.45 0.6 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + false true - 2.50165 1.39644 2.3 0 0 0 + 2.50165 1.39644 2.3 0 -0 0 @@ -2228,6 +3081,13 @@ 0.12 0.8 0.4 + + + + + + + @@ -2240,10 +3100,1599 @@ 0.9 0.9 0.85 1 + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + false + + + + + 0.053484 -0.002297 0.005566 0 -0 0 + 8.2720000000000002 + + 0.048009900000000001 + 0.0038554700000000002 + -0.0112654 + 0.16628999999999999 + -0.00070695999999999997 + 0.16434499999999999 + + + + 0 0 0 0 0 0 + + + 0.3762 0.0935 0.114 + + + + + + 10000 + 1 + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0.33 0 0.08500000000000001 0 0.6109000000000001 0 + + + 0.025 0.09 0.025 + + + + + + + + + + + + + + 0 0 0 0 0 0 + + + 0.001 0.001 0.001 + + + + + + + + + + + + + + 0.25 -0.038 -0.03 2.879 2.775557561562891e-17 1.5705 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/L1_4D_Lidar.dae + + + + + + + + + + + + + 0.2 0 0.11585 0 0 0 + + + 0.0717 + 0.0516 + + + + + + + + + + + + 0 0 0 0 0 0 + + + 0.001 0.001 0.001 + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/trunk.dae + + + + + 0.3381913210524357 0 0.07926395088794351 2.180899851022907 0.0004567769280849116 1.570144029905565 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/d435.dae + + + + + 0 0 0 0 0 0 + + + 0.001 0.001 0.001 + + + + + 0.25 -0.038 -0.03 2.879 2.775557561562891e-17 1.5705 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/L1_4D_Lidar.dae + + + + 0 0 0 1 + 0 0 0 1 + + + + 0.2 0 0.08 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/VLP16_base_1.dae + + + + + 0.2 0 0.08 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/VLP16_base_2.dae + + + + + 0.2 0 0.08 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/VLP16_scan.dae + + + + + 0.33 0 0.085 0 0.6109 0 + rgb_image + 10 + false + + 0 0 0 0 -0 0 + 1.367 + + 640 + 480 + RGB_INT8 + 4 + + __default__ + + false + + 0.050000000000000003 + 200 + + + __default__ + + 4294967295 + + none + 0 + 0 + + + 0 + 0 + 0 + 0 + 0 +
0.5 0.5
+ + + stereographic + true + 1.5708 + 256 + + + + + + 0.346383 0 0.073528 0 0.6109 0 + camera/realsense2_camera_node/depth/image_rect_gazebo_raw + 10 + false + + 0 0 0 0 -0 0 + 1.0469999999999999 + + 480 + 270 + RGB_INT8 + 4 + + __default__ + + false + + 0.01 + 300 + + + depths + + 0.10000000000000001 + 10 + + + + __default__ + + 4294967295 + + gaussian + 0 + 0.0030000000000000001 + + + 0 + 0 + 0 + 0 + 0 +
0.5 0.5
+ + + stereographic + true + 1.5707 + 256 + + 242.47900000000001 + 242.47900000000001 + 242.73599999999999 + 133.273 + 0 + + + + + + + 0.33 0 0.085 0 0.6109 0 + camera/realsense2_camera_node/color/image_raw + 30 + false + + 0 0 0 0 -0 0 + 1.518 + + 640 + 480 + RGB_INT8 + 4 + + __default__ + + false + + 0.050000000000000003 + 10 + + + __default__ + + 4294967295 + + none + 0 + 0 + + + 0 + 0 + 0 + 0 + 0 +
0.5 0.5
+ + + stereographic + true + 1.5708 + 256 + + + + + + 0 0 0 0 -0 0 + imu/data + 100 + false + + + CUSTOM + 0 0 0 + 1 0 0 + + + + + 0 + 0.00020000000000000001 + 0 + 0 + 0 + 0 + 0 + + + + + 0 + 0.00020000000000000001 + 0 + 0 + 0 + 0 + 0 + + + + + 0 + 0.00020000000000000001 + 0 + 0 + 0 + 0 + 0 + + + + + + + 0 + 0.017000000000000001 + 0 + 0 + 0 + 0 + 0 + + + + + 0 + 0.017000000000000001 + 0 + 0 + 0 + 0 + 0 + + + + + 0 + 0.017000000000000001 + 0 + 0 + 0 + 0 + 0 + + + + true + + + false + + + + 0.25 -0.038 -0.03 2.879 0 1.5705 + unitree_lidar + 10 + false + + + + 600 + 1 + -3.1415899999999999 + 3.1415899999999999 + + + 30 + -0.78539800000000004 + 0.78539800000000004 + 1 + + + + 0.80000000000000004 + 30 + 0.001 + + + gaussian + 0 + 0.0080000000000000002 + + 4294967295 + + + + 0.2 0 0.1177 0 -0 0 + scan + 10 + false + + + + 440 + 1 + -3.1415899999999999 + 3.1415899999999999 + + + 16 + -0.261799 + 0.261799 + 1 + + + + 0.029999999999999999 + 131 + 0.001 + + + gaussian + 0 + 0.0080000000000000002 + + 4294967295 + + + 0 0 0 0 -0 0 + false + + + 0 0 0 0 -0 0 + base_link + lf_hip_link + + 1 0 0 + + -1.0471999999999999 + 1.0471999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + 0.1934 0.0465 0 0.037533 -0 0 + + -0.0054 0.00194 -0.000105 0 -0 0 + 0.67800000000000005 + + 0.00048000000000000001 + -3.01e-06 + 1.11e-06 + 0.00088400000000000002 + -1.42e-06 + 0.00059599999999999996 + + + + 0 0.0955 0 1.570796326794896 0 0 + + + 0.040000000000000001 + 0.045999999999999999 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae + + + + false + + + 0 0 0 0 -0 0 + lf_hip_link + lf_upper_leg_link + + 0 1 0 + + -1.5708 + 3.4906999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + 0.1934 0.141933 0.003584 0.074419 1.04176 0.064275 + + -0.00374 -0.0223 -0.0327 0 -0 0 + 1.1519999999999999 + + 0.0058399999999999997 + 8.7200000000000005e-05 + -0.00028899999999999998 + 0.0057999999999999996 + 0.00080800000000000002 + 0.0010300000000000001 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.0245 0.034 + + + + + + 10000 + 1 + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh.dae + + + + true + false + + + 0 0 0 0 -0 0 + lf_upper_leg_link + lf_lower_leg_link + + 0 1 0 + + -2.7227000000000001 + -0.83775999999999995 + 35.549999999999997 + 20.059999999999999 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + 0.009389 0.145958 -0.103621 0.075984 -1.05374 -0.066082 + + 0.003944 -0.000702 -0.142477 0 -0 0 + 0.214 + + 0.00150432 + 5.7069800000000005e-07 + -5.9880799999999998e-06 + 0.00152557 + 1.24056e-05 + 4.3837700000000002e-05 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.016 0.016 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 -0.213 0 0 0 + + + 0.02 + + + + + + 10000 + 1 + + + + + 0.59999999999999998 + 0.59999999999999998 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae + + + + + 0 0 -0.213 0 0 0 + + + 0.01 + + + + true + true + false + + + 0 0 0 0 -0 0 + base_link + lh_hip_link + + 1 0 0 + + -1.0471999999999999 + 1.0471999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + -0.1934 0.0465 -0 -0.004394 -0 0 + + 0.0054 0.00194 -0.000105 0 -0 0 + 0.67800000000000005 + + 0.00048000000000000001 + 3.01e-06 + -1.11e-06 + 0.00088400000000000002 + -1.42e-06 + 0.00059599999999999996 + + + + 0 0.0955 0 1.570796326794896 0 0 + + + 0.040000000000000001 + 0.045999999999999999 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 3.141592653589793 1.224646799147353e-16 3.141592653589793 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae + + + + false + + + 0 0 0 0 -0 0 + lh_hip_link + lh_upper_leg_link + + 0 1 0 + + -1.5708 + 3.4906999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + -0.1934 0.141999 -0.00042 -0.008141 1.00061 -0.006853 + + -0.00374 -0.0223 -0.0327 0 -0 0 + 1.1519999999999999 + + 0.0058399999999999997 + 8.7200000000000005e-05 + -0.00028899999999999998 + 0.0057999999999999996 + 0.00080800000000000002 + 0.0010300000000000001 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.0245 0.034 + + + + + + 10000 + 1 + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh.dae + + + + true + false + + + 0 0 0 0 -0 0 + lh_upper_leg_link + lh_lower_leg_link + + 0 1 0 + + -2.7227000000000001 + -0.83775999999999995 + 35.549999999999997 + 20.059999999999999 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + -0.372706 0.141494 -0.11539 -0.009363 -1.08226 0.008268 + + 0.003944 -0.000702 -0.142477 0 -0 0 + 0.214 + + 0.00150432 + 5.7069800000000005e-07 + -5.9880799999999998e-06 + 0.00152557 + 1.24056e-05 + 4.3837700000000002e-05 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.016 0.016 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 -0.213 0 0 0 + + + 0.02 + + + + + + 10000 + 1 + + + + + 0.59999999999999998 + 0.59999999999999998 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae + + + + + 0 0 -0.213 0 0 0 + + + 0.01 + + + + true + true + false + + + 0 0 0 0 -0 0 + base_link + rf_hip_link + + 1 0 0 + + -1.0471999999999999 + 1.0471999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + 0.1934 -0.0465 -0 -0.036986 -0 0 + + -0.0054 -0.00194 -0.000105 0 -0 0 + 0.67800000000000005 + + 0.00048000000000000001 + 3.01e-06 + 1.11e-06 + 0.00088400000000000002 + 1.42e-06 + 0.00059599999999999996 + + + + 0 -0.0955 0 1.570796326794896 0 0 + + + 0.040000000000000001 + 0.045999999999999999 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 3.141592653589793 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae + + + + false + + + 0 0 0 0 -0 0 + rf_hip_link + rf_upper_leg_link + + 0 1 0 + + -1.5708 + 3.4906999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + 0.1934 -0.141935 0.003531 -0.064745 0.96244 -0.053154 + + -0.00374 0.0223 -0.0327 0 -0 0 + 1.1519999999999999 + + 0.0058399999999999997 + -8.7200000000000005e-05 + -0.00028899999999999998 + 0.0057999999999999996 + -0.00080800000000000002 + 0.0010300000000000001 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.0245 0.034 + + + + + + 10000 + 1 + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh_mirror.dae + + + + true + false + + + 0 0 0 0 -0 0 + rf_upper_leg_link + rf_lower_leg_link + + 0 1 0 + + -2.7227000000000001 + -0.83775999999999995 + 35.549999999999997 + 20.059999999999999 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + 0.018495 -0.14643 -0.117948 -0.091507 -1.1542 0.083719 + + 0.003944 -0.000702 -0.142477 0 -0 0 + 0.214 + + 0.00150432 + 5.7069800000000005e-07 + -5.9880799999999998e-06 + 0.00152557 + 1.24056e-05 + 4.3837700000000002e-05 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.016 0.016 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 -0.213 0 0 0 + + + 0.02 + + + + + + 10000 + 1 + + + + + 0.59999999999999998 + 0.59999999999999998 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae + + + + + 0 0 -0.213 0 0 0 + + + 0.01 + + + + true + true + false + + + 0 0 0 0 -0 0 + base_link + rh_hip_link + + 1 0 0 + + -1.0471999999999999 + 1.0471999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + -0.1934 -0.0465 -0 0.005162 -0 -0 + + 0.0054 -0.00194 -0.000105 0 -0 0 + 0.67800000000000005 + + 0.00048000000000000001 + -3.01e-06 + -1.11e-06 + 0.00088400000000000002 + 1.42e-06 + 0.00059599999999999996 + + + + 0 -0.0955 0 1.570796326794896 0 0 + + + 0.040000000000000001 + 0.045999999999999999 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 -1.224646799147353e-16 1.224646799147353e-16 3.141592653589793 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae + + + + false + + + 0 0 0 0 -0 0 + rh_hip_link + rh_upper_leg_link + + 0 1 0 + + -1.5708 + 3.4906999999999999 + 23.699999999999999 + 30.100000000000001 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + -0.1934 -0.141999 -0.000493 0.010063 1.0322 0.008639 + + -0.00374 0.0223 -0.0327 0 -0 0 + 1.1519999999999999 + + 0.0058399999999999997 + -8.7200000000000005e-05 + -0.00028899999999999998 + 0.0057999999999999996 + -0.00080800000000000002 + 0.0010300000000000001 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.0245 0.034 + + + + + + 10000 + 1 + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh_mirror.dae + + + + true + false + + + 0 0 0 0 -0 0 + rh_upper_leg_link + rh_lower_leg_link + + 0 1 0 + + -2.7227000000000001 + -0.83775999999999995 + 35.549999999999997 + 20.059999999999999 + 100000000 + 1 + + + 0 + 0 + 0.01 + 0.20000000000000001 + + + + + -0.376248 -0.141435 -0.109743 0.010836 -1.07426 -0.009527 + + 0.003944 -0.000702 -0.142477 0 -0 0 + 0.214 + + 0.00150432 + 5.7069800000000005e-07 + -5.9880799999999998e-06 + 0.00152557 + 1.24056e-05 + 4.3837700000000002e-05 + + + + 0 0 -0.1065 0 1.570796326794897 0 + + + 0.213 0.016 0.016 + + + + + + + + + 0.20000000000000001 + 0.20000000000000001 + + + + + + + 0 0 -0.213 0 0 0 + + + 0.02 + + + + + + 10000 + 1 + + + + + 0.59999999999999998 + 0.59999999999999998 + + + + + + + 0 0 0 0 0 0 + + + 1 1 1 + file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae + + + + + 0 0 -0.213 0 0 0 + + + 0.01 + + + + true + true + false + + + true + true + false + false + false + true + true + 100 + + + /home/openmind/unitree-sdk/install/go2_gazebo_sim/share/go2_gazebo_sim/config/ros_control/ros_control.yaml + robot_state_publisher + + + odom + base_link + /odom + 50 + /tf + + + 0.33 0 0.085 0 0.6109 0 + + + + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + + + + 0 0 -0.213 0 -0 0 + + + + 0 0 -0.213 0 -0 0 + + + + 0 0 -0.213 0 -0 0 + + + + 0 0 -0.213 0 -0 0 + + + + 0.25 -0.038 -0.03 2.879 0 1.5705 + + + + 0 0 0.0377 0 -0 0 + + + + 0.2 0 0.08 0 -0 0 + + + -1.35719 4.05052 0.229864 0.014982 0.018088 -0.053436 + false + false + + + true + -4.30296 0.784659 0.3 0 -0 0 + + + + + 1.5 0.4 0.6 + + + + + + + + + + + + + + 1.5 0.4 0.6 + + + + 0.2 0.15 0.1 1 + 0.3 0.25 0.2 1 + + + + false + + + true + -3.20811 0.514428 1.25 0 -0 0 + + + + + 2.5 0.12 2.5 + + + + + + + + + + + + + + 2.5 0.12 2.5 + + + + 0.85 0.85 0.8 1 + 0.9 0.9 0.85 1 + + + false - 0 0 10 0 0 0 + 0 0 10 0 -0 0 true 1 -0.5 0.1 -0.9 @@ -2252,12 +4701,17 @@ 1000 0.01 - 0.9 + 0.90000000000000002 0.001 + + 0 + 0 + 0 + - -2 2 2.4 0 0 0 + -2 2 2.4 0 -0 0 false 1 0 0 -1 @@ -2265,13 +4719,18 @@ 0.1 0.1 0.1 1 10 - 0.1 + 0.10000000000000001 0.5 0.01 + + 0 + 0 + 0 + - 3.5 3 2.4 0 0 0 + 3.5 3 2.4 0 -0 0 false 1 0 0 -1 @@ -2279,13 +4738,18 @@ 0.1 0.1 0.1 1 8 - 0.1 + 0.10000000000000001 0.5 0.01 + + 0 + 0 + 0 + - -2 -2.5 2.4 0 0 0 + -2 -2.5 2.4 0 -0 0 false 1 0 0 -1 @@ -2293,13 +4757,18 @@ 0.2 0.2 0.2 1 6 - 0.1 + 0.10000000000000001 0.5 0.01 + + 0 + 0 + 0 + - 3.5 -2 2.4 0 0 0 + 3.5 -2 2.4 0 -0 0 false 1 0 0 -1 @@ -2307,10 +4776,15 @@ 0.2 0.2 0.2 1 5 - 0.1 + 0.10000000000000001 0.5 0.01 + + 0 + 0 + 0 + - \ No newline at end of file + diff --git a/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml b/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml index fcc876a..26763d8 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml +++ b/go2_gazebo_sim/go2_gazebo_sim/config/gait/gait.yaml @@ -4,9 +4,9 @@ knee_orientation : ">>" pantograph_leg : false odom_scaler: 0.9 - max_linear_velocity_x : 0.3 - max_linear_velocity_y : 0.25 - max_angular_velocity_z : 0.5 + max_linear_velocity_x : 0.6 + max_linear_velocity_y : 0.6 + max_angular_velocity_z : 1.8 com_x_translation: 0.0 swing_height : 0.04 stance_depth : 0.01 diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index e20c9ac..6f9c669 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -30,7 +30,7 @@ amcl: save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: True - transform_tolerance: 0.5 # Reduced from 1.0 + transform_tolerance: 1.0 update_min_a: 0.005 update_min_d: 0.02 z_hit: 0.85 @@ -48,11 +48,11 @@ bt_navigator: ros__parameters: use_sim_time: False global_frame: map - transform_tolerance: 0.5 # Reduced from 1.0 + transform_tolerance: 1.0 robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 - default_server_timeout: 20 + default_server_timeout: 30 enable_groot_monitoring: True groot_zmq_publisher_port: 1666 groot_zmq_server_port: 1667 @@ -107,88 +107,85 @@ bt_navigator: controller_server: ros__parameters: use_sim_time: False - controller_frequency: 40.0 - min_x_velocity_threshold: 0.001 + controller_frequency: 30.0 + min_x_velocity_threshold: 0.05 min_y_velocity_threshold: 0.001 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.5 + min_theta_velocity_threshold: 0.1 + failure_tolerance: 0.8 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] controller_plugins: ["FollowPath"] progress_checker: plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.3 # Increased slightly - movement_time_allowance: 20.0 # Increased from 15.0 + required_movement_radius: 0.1 + movement_time_allowance: 45.0 general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 + xy_goal_tolerance: 0.35 + yaw_goal_tolerance: 0.35 - # MPPI Controller - optimized for Unitree Go2 FollowPath: plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 # Prediction horizon steps - model_dt: 0.05 # Time between steps (50ms) - batch_size: 2000 - vx_std: 0.2 # Std dev for forward velocity sampling - vy_std: 0.0 # No lateral movement for quadruped - wz_std: 0.4 # Std dev for rotational velocity sampling - vx_max: 0.4 # Max forward velocity - vx_min: -0.3 # Max backward velocity + time_steps: 48 + model_dt: 0.07 + batch_size: 1500 + vx_std: 0.2 # Velocity variance (exploration noise) + vy_std: 0.0 + wz_std: 0.4 # Angular velocity variance + vx_max: 0.5 # Absolute max forward speed (m/s) + vx_min: -0.35 # Max reverse speed (m/s) vy_max: 0.0 - wz_max: 1.2 # Max rotation rate - iteration_count: 1 # MPPI iterations per control cycle - prune_distance: 1.7 # Distance ahead to prune path - transform_tolerance: 0.3 - temperature: 0.3 # Lower = more exploitation vs exploration - gamma: 0.015 # Discount factor - motion_model: "DiffDrive" # Use DiffDrive for Go2 - visualize: False # Set to True for debugging - regenerate_noises: False - - # Cost function weights (tune these for your needs) - critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] - - ConstraintCritic: + wz_max: 1.2 # Max angular velocity (rad/s) + iteration_count: 1 # Optimization iterations (1 is efficient) + prune_distance: 1.5 # Remove path points closer than this + transform_tolerance: 0.5 + temperature: 0.4 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: False + regenerate_noises: True + + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic", "ObstaclesCritic"] + + ConstraintCritic: # Penalizes violating velocity/accel limits enabled: True cost_power: 1 cost_weight: 4.0 - ObstaclesCritic: + GoalCritic: # Penalizes distance to goal enabled: True cost_power: 1 - cost_weight: 20.0 - consider_footprint: False - collision_cost: 10000.0 - collision_margin_distance: 0.1 - near_goal_distance: 0.5 + cost_weight: 10.0 + threshold_to_consider: 1.2 - GoalCritic: + ObstaclesCritic: # Penalizes collisions (CRITICAL) enabled: True cost_power: 1 - cost_weight: 5.0 # High weight for reaching goal - threshold_to_consider: 1.4 + cost_weight: 10.0 # Reduced from 18.0 to allow closer approach + consider_footprint: False + collision_cost: 10000.0 + collision_margin_distance: 0.05 # Reduced from 0.12 for tighter navigation + near_goal_distance: 0.4 GoalAngleCritic: enabled: True - cost_power: 1 - cost_weight: 15.0 # Important for final orientation - threshold_to_consider: 0.5 + cost_power: 4 + cost_weight: 10.0 + threshold_to_consider: 0.6 PreferForwardCritic: enabled: True cost_power: 1 - cost_weight: 1.0 # Penalize backward motion + cost_weight: 16.0 threshold_to_consider: 0.5 - # Obstacle and path following - CostCritic: + CostCritic: # Penalizes high-cost areas enabled: True cost_power: 1 - cost_weight: 3.81 + cost_weight: 3.5 critical_cost: 300.0 consider_footprint: True collision_cost: 1000000.0 @@ -198,52 +195,53 @@ controller_server: PathAlignCritic: enabled: True cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 + cost_weight: 32.0 + max_path_occupancy_ratio: 0.07 + trajectory_point_step: 3 + threshold_to_consider: 0.6 + offset_from_furthest: 15 use_path_orientations: False PathFollowCritic: enabled: True cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 + cost_weight: 20.0 + offset_from_furthest: 6 + threshold_to_consider: 1.2 PathAngleCritic: enabled: True cost_power: 1 cost_weight: 2.0 offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 + threshold_to_consider: 0.6 + max_angle_to_furthest: 1.2 mode: 0 - # Noise generator settings noise_generator_config: default: - time_steps: 56 - batch_size: 2000 - vx_std: 0.2 - wz_std: 0.4 + time_steps: 48 + batch_size: 1500 + vx_std: 0.15 + wz_std: 0.35 vx_mu: 0.0 wz_mu: 0.0 local_costmap: local_costmap: ros__parameters: - update_frequency: 10.0 - publish_frequency: 5.0 + update_frequency: 8.0 + publish_frequency: 4.0 global_frame: odom robot_base_frame: base_link use_sim_time: False rolling_window: True - width: 3 - height: 3 + width: 4 + height: 4 resolution: 0.05 - footprint: "[[0.3, 0.2], [0.3, -0.45], [-0.3, -0.45], [-0.3, 0.2]]" + transform_tolerance: 1.0 + # Slightly smaller footprint to prevent self-collision in costmap + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" # Robot shape [x,y] for collision checking plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] static_layer: @@ -253,92 +251,96 @@ local_costmap: obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + footprint_clearing_enabled: True # Clear footprint area + max_obstacle_height: 2.0 observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 + min_obstacle_height: 0.0 clearing: True marking: True data_type: "LaserScan" - raytrace_max_range: 3.0 + raytrace_max_range: 3.5 raytrace_min_range: 0.0 - obstacle_max_range: 2.5 - obstacle_min_range: 0.0 + obstacle_max_range: 3.0 + obstacle_min_range: 0.15 - # STVL for 4D LiDAR pointcloud (optimized for ground obstacles) stvl_layer: plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" enabled: True - voxel_decay: 0.8 # Faster decay for dynamic ground obstacles - decay_model: 0 # 0=linear, 1=exponential, 2=persistent - voxel_size: 0.04 # 4cm voxels for better ground detail - track_unknown_space: True + voxel_decay: 1.5 # Faster decay + decay_model: 0 + voxel_size: 0.05 + track_unknown_space: False observation_persistence: 0.0 - max_obstacle_height: 2.5 # Full height detection with 4D LiDAR - min_obstacle_height: 0.15 # 15.0cm - lower threshold for ground obstacles - obstacle_max_range: 2.5 # Focused range for ground detection - obstacle_min_range: 0.25 # Minimum range to avoid self-detection + max_obstacle_height: 2.5 + min_obstacle_height: 0.18 # Slightly higher to avoid ground noise + obstacle_max_range: 2.5 + obstacle_min_range: 0.25 origin_z: 0.0 publish_voxel_map: True - transform_tolerance: 0.5 + transform_tolerance: 1.0 mapping_mode: False map_save_duration: 60.0 - combination_method: 1 # 0=max, 1=override + combination_method: 1 update_footprint_enabled: True - - # 4D LiDAR as observation source observation_sources: lidar_4d - - # 4D LiDAR pointcloud configuration (Unitree Go2) lidar_4d: data_type: PointCloud2 - topic: /utlidar/cloud_deskewed # Motion-compensated pointcloud + topic: /utlidar/cloud_deskewed marking: True clearing: True - min_obstacle_height: 0.15 # 15.0cm threshold for ground obstacles - max_obstacle_height: 2.5 # Full height coverage - expected_update_rate: 15.0 # Unitree 4D LiDAR at ~15Hz + min_obstacle_height: 0.18 + max_obstacle_height: 2.5 + expected_update_rate: 15.0 observation_persistence: 0.0 inf_is_valid: False filter: "voxel" - voxel_min_points: 1 # Lower threshold for ground obstacle sensitivity + voxel_min_points: 3 clear_after_reading: True enabled: True - model_type: 1 # 1=3d_lidar (for 4D LiDAR) + model_type: 1 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 5.0 - inflation_radius: 0.4 + cost_scaling_factor: 5.0 # Faster decay (less repulsion) + inflation_radius: 0.30 # Original value + always_send_full_costmap: True global_costmap: global_costmap: ros__parameters: - update_frequency: 5.0 - publish_frequency: 2.0 + update_frequency: 2.0 # Reduced + publish_frequency: 1.0 global_frame: map robot_base_frame: base_link use_sim_time: False - footprint: "[[0.3, 0.2], [0.3, -0.2], [-0.3, -0.2], [-0.3, 0.2]]" + transform_tolerance: 1.0 + # Slightly smaller footprint + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" resolution: 0.1 - track_unknown_space: True + track_unknown_space: False plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True + footprint_clearing_enabled: True + max_obstacle_height: 2.0 observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 + min_obstacle_height: 0.0 clearing: True marking: True data_type: "LaserScan" - raytrace_max_range: 3.0 + raytrace_max_range: 4.0 raytrace_min_range: 0.0 obstacle_max_range: 3.5 - obstacle_min_range: 0.0 + obstacle_min_range: 0.15 static_layer: plugin: "nav2_costmap_2d::StaticLayer" @@ -346,8 +348,8 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 5.0 - inflation_radius: 0.5 + cost_scaling_factor: 6.0 # Faster decay (less repulsion) + inflation_radius: 0.32 # Original value always_send_full_costmap: True @@ -367,58 +369,20 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 5.0 - transform_tolerance: 0.1 + transform_tolerance: 1.0 use_sim_time: False planner_plugins: ["GridBased"] - # SmacPlannerHybrid with Reeds-Shepp for backward motion in narrow lanes GridBased: - plugin: "nav2_smac_planner/SmacPlannerHybrid" - tolerance: 0.3 + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.5 downsample_costmap: False downsampling_factor: 1 - allow_unknown: True + allow_unknown: True # Allow planning through unknown space max_iterations: 1000000 max_on_approach_iterations: 1000 - max_planning_time: 5.0 smooth_path: True - - # Reeds-Shepp motion model - enables forward AND backward motion - motion_model_for_search: "REEDS_SHEPP" - - # Angular resolution (72 bins = 5 degree resolution) - angle_quantization_bins: 72 - - # Analytic expansion parameters - analytic_expansion_ratio: 3.5 - analytic_expansion_max_length: 3.0 - - # Minimum turning radius (meters) - tuned for Go2's turning capability - minimum_turning_radius: 0.1 - - # Reverse penalty - lower value encourages reverse when beneficial for narrow lanes - reverse_penalty: 0.1 - - # Change of direction penalty - low to allow direction switches in tight spaces - change_penalty: 5.15 - non_straight_penalty: 0.4 - - # Cost penalties - cost_penalty: 2.0 - retrospective_penalty: 0.015 - - # Lookup table size for hybrid search - lookup_table_size: 20.0 - - # Cache obstacle heuristic for performance - cache_obstacle_heuristic: False - - # Smoothing parameters - smoother: - max_iterations: 1000 - w_smooth: 0.3 - w_data: 0.2 - tolerance: 1.0e-10 - do_refinement: True + #Add cost_travel_multiplier to prefer free space + cost_travel_multiplier: 2.0 # Higher = prefer low cost areas vs short path smoother_server: ros__parameters: @@ -440,9 +404,9 @@ behavior_server: plugin: "nav2_behaviors/Spin" backup: plugin: "nav2_behaviors/BackUp" - backup_dist: 0.5 - backup_speed: 0.15 - time_allowance: 10.0 + backup_dist: 0.4 + backup_speed: 0.12 + time_allowance: 20.0 drive_on_heading: plugin: "nav2_behaviors/DriveOnHeading" wait: @@ -451,12 +415,12 @@ behavior_server: plugin: "nav2_behaviors/AssistedTeleop" global_frame: odom robot_base_frame: base_link - transform_tolerance: 0.5 + transform_tolerance: 1.0 # Increased use_sim_time: False - simulate_ahead_time: 2.5 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 + simulate_ahead_time: 3.0 + max_rotational_vel: 0.6 + min_rotational_vel: 0.15 + rotational_acc_lim: 1.8 robot_state_publisher: ros__parameters: @@ -479,10 +443,10 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.4, 0.0, 1.2] # Updated for MPPI limits - min_velocity: [-0.3, 0.0, -1.2] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] + max_velocity: [0.5, 0.0, 1.2] # Kinematic hard limits [vx, vy, wz] + min_velocity: [-0.35, 0.0, -1.2] + max_accel: [2.0, 0.0, 2.5] # Max acceleration [ax, ay, az] + max_decel: [-2.0, 0.0, -2.5] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] diff --git a/go2_sdk/config/nav2_params_sim.yaml b/go2_sdk/config/nav2_params_sim.yaml new file mode 100644 index 0000000..6f9c669 --- /dev/null +++ b/go2_sdk/config/nav2_params_sim.yaml @@ -0,0 +1,453 @@ +amcl: + ros__parameters: + use_sim_time: False + alpha1: 0.05 + alpha2: 0.1 + alpha3: 0.05 + alpha4: 0.05 + alpha5: 0.05 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: False + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 12.0 + laser_min_range: 0.1 + laser_model_type: "likelihood_field" + max_beams: 180 + max_particles: 8000 + min_particles: 2000 + odom_frame_id: "odom" + pf_err: 0.01 + pf_z: 0.95 + recovery_alpha_fast: 0.3 + recovery_alpha_slow: 0.01 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: True + transform_tolerance: 1.0 + update_min_a: 0.005 + update_min_d: 0.02 + z_hit: 0.85 + z_max: 0.05 + z_rand: 0.05 + z_short: 0.15 + scan_topic: scan + set_initial_pose: True + initial_pose_x: 0.0 + initial_pose_y: 0.0 + initial_pose_z: 0.0 + initial_pose_a: 0.0 + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + transform_tolerance: 1.0 + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 30 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 30.0 + min_x_velocity_threshold: 0.05 + min_y_velocity_threshold: 0.001 + min_theta_velocity_threshold: 0.1 + failure_tolerance: 0.8 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.1 + movement_time_allowance: 45.0 + + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.35 + yaw_goal_tolerance: 0.35 + + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 48 + model_dt: 0.07 + batch_size: 1500 + vx_std: 0.2 # Velocity variance (exploration noise) + vy_std: 0.0 + wz_std: 0.4 # Angular velocity variance + vx_max: 0.5 # Absolute max forward speed (m/s) + vx_min: -0.35 # Max reverse speed (m/s) + vy_max: 0.0 + wz_max: 1.2 # Max angular velocity (rad/s) + iteration_count: 1 # Optimization iterations (1 is efficient) + prune_distance: 1.5 # Remove path points closer than this + transform_tolerance: 0.5 + temperature: 0.4 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: False + regenerate_noises: True + + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic", "ObstaclesCritic"] + + ConstraintCritic: # Penalizes violating velocity/accel limits + enabled: True + cost_power: 1 + cost_weight: 4.0 + + GoalCritic: # Penalizes distance to goal + enabled: True + cost_power: 1 + cost_weight: 10.0 + threshold_to_consider: 1.2 + + ObstaclesCritic: # Penalizes collisions (CRITICAL) + enabled: True + cost_power: 1 + cost_weight: 10.0 # Reduced from 18.0 to allow closer approach + consider_footprint: False + collision_cost: 10000.0 + collision_margin_distance: 0.05 # Reduced from 0.12 for tighter navigation + near_goal_distance: 0.4 + + GoalAngleCritic: + enabled: True + cost_power: 4 + cost_weight: 10.0 + threshold_to_consider: 0.6 + + PreferForwardCritic: + enabled: True + cost_power: 1 + cost_weight: 16.0 + threshold_to_consider: 0.5 + + CostCritic: # Penalizes high-cost areas + enabled: True + cost_power: 1 + cost_weight: 3.5 + critical_cost: 300.0 + consider_footprint: True + collision_cost: 1000000.0 + near_goal_distance: 0.5 + trajectory_point_step: 2 + + PathAlignCritic: + enabled: True + cost_power: 1 + cost_weight: 32.0 + max_path_occupancy_ratio: 0.07 + trajectory_point_step: 3 + threshold_to_consider: 0.6 + offset_from_furthest: 15 + use_path_orientations: False + + PathFollowCritic: + enabled: True + cost_power: 1 + cost_weight: 20.0 + offset_from_furthest: 6 + threshold_to_consider: 1.2 + + PathAngleCritic: + enabled: True + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.6 + max_angle_to_furthest: 1.2 + mode: 0 + + noise_generator_config: + default: + time_steps: 48 + batch_size: 1500 + vx_std: 0.15 + wz_std: 0.35 + vx_mu: 0.0 + wz_mu: 0.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 8.0 + publish_frequency: 4.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: False + rolling_window: True + width: 4 + height: 4 + resolution: 0.05 + transform_tolerance: 1.0 + # Slightly smaller footprint to prevent self-collision in costmap + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" # Robot shape [x,y] for collision checking + plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] + + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + footprint_clearing_enabled: True # Clear footprint area + max_obstacle_height: 2.0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.5 + raytrace_min_range: 0.0 + obstacle_max_range: 3.0 + obstacle_min_range: 0.15 + + stvl_layer: + plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" + enabled: True + voxel_decay: 1.5 # Faster decay + decay_model: 0 + voxel_size: 0.05 + track_unknown_space: False + observation_persistence: 0.0 + max_obstacle_height: 2.5 + min_obstacle_height: 0.18 # Slightly higher to avoid ground noise + obstacle_max_range: 2.5 + obstacle_min_range: 0.25 + origin_z: 0.0 + publish_voxel_map: True + transform_tolerance: 1.0 + mapping_mode: False + map_save_duration: 60.0 + combination_method: 1 + update_footprint_enabled: True + observation_sources: lidar_4d + lidar_4d: + data_type: PointCloud2 + topic: /utlidar/cloud_deskewed + marking: True + clearing: True + min_obstacle_height: 0.18 + max_obstacle_height: 2.5 + expected_update_rate: 15.0 + observation_persistence: 0.0 + inf_is_valid: False + filter: "voxel" + voxel_min_points: 3 + clear_after_reading: True + enabled: True + model_type: 1 + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 5.0 # Faster decay (less repulsion) + inflation_radius: 0.30 # Original value + + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 2.0 # Reduced + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: False + transform_tolerance: 1.0 + # Slightly smaller footprint + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" + resolution: 0.1 + track_unknown_space: False + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + footprint_clearing_enabled: True + max_obstacle_height: 2.0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 4.0 + raytrace_min_range: 0.0 + obstacle_max_range: 3.5 + obstacle_min_range: 0.15 + + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 6.0 # Faster decay (less repulsion) + inflation_radius: 0.32 # Original value + + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: False + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: False + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 5.0 + transform_tolerance: 1.0 + use_sim_time: False + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.5 + downsample_costmap: False + downsampling_factor: 1 + allow_unknown: True # Allow planning through unknown space + max_iterations: 1000000 + max_on_approach_iterations: 1000 + smooth_path: True + #Add cost_travel_multiplier to prefer free space + cost_travel_multiplier: 2.0 # Higher = prefer low cost areas vs short path + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.4 + backup_speed: 0.12 + time_allowance: 20.0 + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 1.0 # Increased + use_sim_time: False + simulate_ahead_time: 3.0 + max_rotational_vel: 0.6 + min_rotational_vel: 0.15 + rotational_acc_lim: 1.8 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + use_sim_time: False + loop_rate: 20 + stop_on_failure: False + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 1.2] # Kinematic hard limits [vx, vy, wz] + min_velocity: [-0.35, 0.0, -1.2] + max_accel: [2.0, 0.0, 2.5] # Max acceleration [ax, ay, az] + max_decel: [-2.0, 0.0, -2.5] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file From 14a9a70e974afee4cc3c85beb7e2130cae0e41e7 Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Wed, 10 Dec 2025 15:33:53 -0800 Subject: [PATCH 03/13] Update Nav params for real robot, cleanup world file, update launch files to use sim params if use_sim is true --- .../go2_description/worlds/home_world.sdf | 1510 ----------------- go2_sdk/config/nav2_params.yaml | 38 +- go2_sdk/launch/nav2_launch.py | 12 +- go2_sdk/launch/slam_launch.py | 10 +- m-explore-ros2 | 1 + 5 files changed, 37 insertions(+), 1534 deletions(-) create mode 160000 m-explore-ros2 diff --git a/go2_gazebo_sim/go2_description/worlds/home_world.sdf b/go2_gazebo_sim/go2_description/worlds/home_world.sdf index 85a30af..150f657 100644 --- a/go2_gazebo_sim/go2_description/worlds/home_world.sdf +++ b/go2_gazebo_sim/go2_description/worlds/home_world.sdf @@ -3117,1516 +3117,6 @@ false - - - - 0.053484 -0.002297 0.005566 0 -0 0 - 8.2720000000000002 - - 0.048009900000000001 - 0.0038554700000000002 - -0.0112654 - 0.16628999999999999 - -0.00070695999999999997 - 0.16434499999999999 - - - - 0 0 0 0 0 0 - - - 0.3762 0.0935 0.114 - - - - - - 10000 - 1 - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0.33 0 0.08500000000000001 0 0.6109000000000001 0 - - - 0.025 0.09 0.025 - - - - - - - - - - - - - - 0 0 0 0 0 0 - - - 0.001 0.001 0.001 - - - - - - - - - - - - - - 0.25 -0.038 -0.03 2.879 2.775557561562891e-17 1.5705 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/L1_4D_Lidar.dae - - - - - - - - - - - - - 0.2 0 0.11585 0 0 0 - - - 0.0717 - 0.0516 - - - - - - - - - - - - 0 0 0 0 0 0 - - - 0.001 0.001 0.001 - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/trunk.dae - - - - - 0.3381913210524357 0 0.07926395088794351 2.180899851022907 0.0004567769280849116 1.570144029905565 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/d435.dae - - - - - 0 0 0 0 0 0 - - - 0.001 0.001 0.001 - - - - - 0.25 -0.038 -0.03 2.879 2.775557561562891e-17 1.5705 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/L1_4D_Lidar.dae - - - - 0 0 0 1 - 0 0 0 1 - - - - 0.2 0 0.08 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/VLP16_base_1.dae - - - - - 0.2 0 0.08 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/VLP16_base_2.dae - - - - - 0.2 0 0.08 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/VLP16_scan.dae - - - - - 0.33 0 0.085 0 0.6109 0 - rgb_image - 10 - false - - 0 0 0 0 -0 0 - 1.367 - - 640 - 480 - RGB_INT8 - 4 - - __default__ - - false - - 0.050000000000000003 - 200 - - - __default__ - - 4294967295 - - none - 0 - 0 - - - 0 - 0 - 0 - 0 - 0 -
0.5 0.5
- - - stereographic - true - 1.5708 - 256 - - - - - - 0.346383 0 0.073528 0 0.6109 0 - camera/realsense2_camera_node/depth/image_rect_gazebo_raw - 10 - false - - 0 0 0 0 -0 0 - 1.0469999999999999 - - 480 - 270 - RGB_INT8 - 4 - - __default__ - - false - - 0.01 - 300 - - - depths - - 0.10000000000000001 - 10 - - - - __default__ - - 4294967295 - - gaussian - 0 - 0.0030000000000000001 - - - 0 - 0 - 0 - 0 - 0 -
0.5 0.5
- - - stereographic - true - 1.5707 - 256 - - 242.47900000000001 - 242.47900000000001 - 242.73599999999999 - 133.273 - 0 - - - - - - - 0.33 0 0.085 0 0.6109 0 - camera/realsense2_camera_node/color/image_raw - 30 - false - - 0 0 0 0 -0 0 - 1.518 - - 640 - 480 - RGB_INT8 - 4 - - __default__ - - false - - 0.050000000000000003 - 10 - - - __default__ - - 4294967295 - - none - 0 - 0 - - - 0 - 0 - 0 - 0 - 0 -
0.5 0.5
- - - stereographic - true - 1.5708 - 256 - - - - - - 0 0 0 0 -0 0 - imu/data - 100 - false - - - CUSTOM - 0 0 0 - 1 0 0 - - - - - 0 - 0.00020000000000000001 - 0 - 0 - 0 - 0 - 0 - - - - - 0 - 0.00020000000000000001 - 0 - 0 - 0 - 0 - 0 - - - - - 0 - 0.00020000000000000001 - 0 - 0 - 0 - 0 - 0 - - - - - - - 0 - 0.017000000000000001 - 0 - 0 - 0 - 0 - 0 - - - - - 0 - 0.017000000000000001 - 0 - 0 - 0 - 0 - 0 - - - - - 0 - 0.017000000000000001 - 0 - 0 - 0 - 0 - 0 - - - - true - - - false - - - - 0.25 -0.038 -0.03 2.879 0 1.5705 - unitree_lidar - 10 - false - - - - 600 - 1 - -3.1415899999999999 - 3.1415899999999999 - - - 30 - -0.78539800000000004 - 0.78539800000000004 - 1 - - - - 0.80000000000000004 - 30 - 0.001 - - - gaussian - 0 - 0.0080000000000000002 - - 4294967295 - - - - 0.2 0 0.1177 0 -0 0 - scan - 10 - false - - - - 440 - 1 - -3.1415899999999999 - 3.1415899999999999 - - - 16 - -0.261799 - 0.261799 - 1 - - - - 0.029999999999999999 - 131 - 0.001 - - - gaussian - 0 - 0.0080000000000000002 - - 4294967295 - - - 0 0 0 0 -0 0 - false - - - 0 0 0 0 -0 0 - base_link - lf_hip_link - - 1 0 0 - - -1.0471999999999999 - 1.0471999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - 0.1934 0.0465 0 0.037533 -0 0 - - -0.0054 0.00194 -0.000105 0 -0 0 - 0.67800000000000005 - - 0.00048000000000000001 - -3.01e-06 - 1.11e-06 - 0.00088400000000000002 - -1.42e-06 - 0.00059599999999999996 - - - - 0 0.0955 0 1.570796326794896 0 0 - - - 0.040000000000000001 - 0.045999999999999999 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae - - - - false - - - 0 0 0 0 -0 0 - lf_hip_link - lf_upper_leg_link - - 0 1 0 - - -1.5708 - 3.4906999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - 0.1934 0.141933 0.003584 0.074419 1.04176 0.064275 - - -0.00374 -0.0223 -0.0327 0 -0 0 - 1.1519999999999999 - - 0.0058399999999999997 - 8.7200000000000005e-05 - -0.00028899999999999998 - 0.0057999999999999996 - 0.00080800000000000002 - 0.0010300000000000001 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.0245 0.034 - - - - - - 10000 - 1 - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh.dae - - - - true - false - - - 0 0 0 0 -0 0 - lf_upper_leg_link - lf_lower_leg_link - - 0 1 0 - - -2.7227000000000001 - -0.83775999999999995 - 35.549999999999997 - 20.059999999999999 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - 0.009389 0.145958 -0.103621 0.075984 -1.05374 -0.066082 - - 0.003944 -0.000702 -0.142477 0 -0 0 - 0.214 - - 0.00150432 - 5.7069800000000005e-07 - -5.9880799999999998e-06 - 0.00152557 - 1.24056e-05 - 4.3837700000000002e-05 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.016 0.016 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 -0.213 0 0 0 - - - 0.02 - - - - - - 10000 - 1 - - - - - 0.59999999999999998 - 0.59999999999999998 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae - - - - - 0 0 -0.213 0 0 0 - - - 0.01 - - - - true - true - false - - - 0 0 0 0 -0 0 - base_link - lh_hip_link - - 1 0 0 - - -1.0471999999999999 - 1.0471999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - -0.1934 0.0465 -0 -0.004394 -0 0 - - 0.0054 0.00194 -0.000105 0 -0 0 - 0.67800000000000005 - - 0.00048000000000000001 - 3.01e-06 - -1.11e-06 - 0.00088400000000000002 - -1.42e-06 - 0.00059599999999999996 - - - - 0 0.0955 0 1.570796326794896 0 0 - - - 0.040000000000000001 - 0.045999999999999999 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 3.141592653589793 1.224646799147353e-16 3.141592653589793 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae - - - - false - - - 0 0 0 0 -0 0 - lh_hip_link - lh_upper_leg_link - - 0 1 0 - - -1.5708 - 3.4906999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - -0.1934 0.141999 -0.00042 -0.008141 1.00061 -0.006853 - - -0.00374 -0.0223 -0.0327 0 -0 0 - 1.1519999999999999 - - 0.0058399999999999997 - 8.7200000000000005e-05 - -0.00028899999999999998 - 0.0057999999999999996 - 0.00080800000000000002 - 0.0010300000000000001 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.0245 0.034 - - - - - - 10000 - 1 - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh.dae - - - - true - false - - - 0 0 0 0 -0 0 - lh_upper_leg_link - lh_lower_leg_link - - 0 1 0 - - -2.7227000000000001 - -0.83775999999999995 - 35.549999999999997 - 20.059999999999999 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - -0.372706 0.141494 -0.11539 -0.009363 -1.08226 0.008268 - - 0.003944 -0.000702 -0.142477 0 -0 0 - 0.214 - - 0.00150432 - 5.7069800000000005e-07 - -5.9880799999999998e-06 - 0.00152557 - 1.24056e-05 - 4.3837700000000002e-05 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.016 0.016 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 -0.213 0 0 0 - - - 0.02 - - - - - - 10000 - 1 - - - - - 0.59999999999999998 - 0.59999999999999998 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae - - - - - 0 0 -0.213 0 0 0 - - - 0.01 - - - - true - true - false - - - 0 0 0 0 -0 0 - base_link - rf_hip_link - - 1 0 0 - - -1.0471999999999999 - 1.0471999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - 0.1934 -0.0465 -0 -0.036986 -0 0 - - -0.0054 -0.00194 -0.000105 0 -0 0 - 0.67800000000000005 - - 0.00048000000000000001 - 3.01e-06 - 1.11e-06 - 0.00088400000000000002 - 1.42e-06 - 0.00059599999999999996 - - - - 0 -0.0955 0 1.570796326794896 0 0 - - - 0.040000000000000001 - 0.045999999999999999 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 3.141592653589793 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae - - - - false - - - 0 0 0 0 -0 0 - rf_hip_link - rf_upper_leg_link - - 0 1 0 - - -1.5708 - 3.4906999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - 0.1934 -0.141935 0.003531 -0.064745 0.96244 -0.053154 - - -0.00374 0.0223 -0.0327 0 -0 0 - 1.1519999999999999 - - 0.0058399999999999997 - -8.7200000000000005e-05 - -0.00028899999999999998 - 0.0057999999999999996 - -0.00080800000000000002 - 0.0010300000000000001 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.0245 0.034 - - - - - - 10000 - 1 - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh_mirror.dae - - - - true - false - - - 0 0 0 0 -0 0 - rf_upper_leg_link - rf_lower_leg_link - - 0 1 0 - - -2.7227000000000001 - -0.83775999999999995 - 35.549999999999997 - 20.059999999999999 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - 0.018495 -0.14643 -0.117948 -0.091507 -1.1542 0.083719 - - 0.003944 -0.000702 -0.142477 0 -0 0 - 0.214 - - 0.00150432 - 5.7069800000000005e-07 - -5.9880799999999998e-06 - 0.00152557 - 1.24056e-05 - 4.3837700000000002e-05 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.016 0.016 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 -0.213 0 0 0 - - - 0.02 - - - - - - 10000 - 1 - - - - - 0.59999999999999998 - 0.59999999999999998 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae - - - - - 0 0 -0.213 0 0 0 - - - 0.01 - - - - true - true - false - - - 0 0 0 0 -0 0 - base_link - rh_hip_link - - 1 0 0 - - -1.0471999999999999 - 1.0471999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - -0.1934 -0.0465 -0 0.005162 -0 -0 - - 0.0054 -0.00194 -0.000105 0 -0 0 - 0.67800000000000005 - - 0.00048000000000000001 - -3.01e-06 - -1.11e-06 - 0.00088400000000000002 - 1.42e-06 - 0.00059599999999999996 - - - - 0 -0.0955 0 1.570796326794896 0 0 - - - 0.040000000000000001 - 0.045999999999999999 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 -1.224646799147353e-16 1.224646799147353e-16 3.141592653589793 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/hip.dae - - - - false - - - 0 0 0 0 -0 0 - rh_hip_link - rh_upper_leg_link - - 0 1 0 - - -1.5708 - 3.4906999999999999 - 23.699999999999999 - 30.100000000000001 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - -0.1934 -0.141999 -0.000493 0.010063 1.0322 0.008639 - - -0.00374 0.0223 -0.0327 0 -0 0 - 1.1519999999999999 - - 0.0058399999999999997 - -8.7200000000000005e-05 - -0.00028899999999999998 - 0.0057999999999999996 - -0.00080800000000000002 - 0.0010300000000000001 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.0245 0.034 - - - - - - 10000 - 1 - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/thigh_mirror.dae - - - - true - false - - - 0 0 0 0 -0 0 - rh_upper_leg_link - rh_lower_leg_link - - 0 1 0 - - -2.7227000000000001 - -0.83775999999999995 - 35.549999999999997 - 20.059999999999999 - 100000000 - 1 - - - 0 - 0 - 0.01 - 0.20000000000000001 - - - - - -0.376248 -0.141435 -0.109743 0.010836 -1.07426 -0.009527 - - 0.003944 -0.000702 -0.142477 0 -0 0 - 0.214 - - 0.00150432 - 5.7069800000000005e-07 - -5.9880799999999998e-06 - 0.00152557 - 1.24056e-05 - 4.3837700000000002e-05 - - - - 0 0 -0.1065 0 1.570796326794897 0 - - - 0.213 0.016 0.016 - - - - - - - - - 0.20000000000000001 - 0.20000000000000001 - - - - - - - 0 0 -0.213 0 0 0 - - - 0.02 - - - - - - 10000 - 1 - - - - - 0.59999999999999998 - 0.59999999999999998 - - - - - - - 0 0 0 0 0 0 - - - 1 1 1 - file:///home/openmind/unitree-sdk/install/go2_description/share/go2_description/meshes/calf.dae - - - - - 0 0 -0.213 0 0 0 - - - 0.01 - - - - true - true - false - - - true - true - false - false - false - true - true - 100 - - - /home/openmind/unitree-sdk/install/go2_gazebo_sim/share/go2_gazebo_sim/config/ros_control/ros_control.yaml - robot_state_publisher - - - odom - base_link - /odom - 50 - /tf - - - 0.33 0 0.085 0 0.6109 0 - - - - 0 0 0 0 -0 0 - - - - 0 0 0 0 -0 0 - - - - 0 0 -0.213 0 -0 0 - - - - 0 0 -0.213 0 -0 0 - - - - 0 0 -0.213 0 -0 0 - - - - 0 0 -0.213 0 -0 0 - - - - 0.25 -0.038 -0.03 2.879 0 1.5705 - - - - 0 0 0.0377 0 -0 0 - - - - 0.2 0 0.08 0 -0 0 - - - -1.35719 4.05052 0.229864 0.014982 0.018088 -0.053436 - false - false - true -4.30296 0.784659 0.3 0 -0 0 diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index 6f9c669..04c18ac 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -132,13 +132,13 @@ controller_server: time_steps: 48 model_dt: 0.07 batch_size: 1500 - vx_std: 0.2 # Velocity variance (exploration noise) + vx_std: 0.15 # Velocity variance (exploration noise) vy_std: 0.0 - wz_std: 0.4 # Angular velocity variance - vx_max: 0.5 # Absolute max forward speed (m/s) - vx_min: -0.35 # Max reverse speed (m/s) + wz_std: 0.35 # Angular velocity variance + vx_max: 0.25 # Absolute max forward speed (m/s) + vx_min: -0.22 # Max reverse speed (m/s) vy_max: 0.0 - wz_max: 1.2 # Max angular velocity (rad/s) + wz_max: 0.8 # Max angular velocity (rad/s) iteration_count: 1 # Optimization iterations (1 is efficient) prune_distance: 1.5 # Remove path points closer than this transform_tolerance: 0.5 @@ -164,22 +164,22 @@ controller_server: ObstaclesCritic: # Penalizes collisions (CRITICAL) enabled: True cost_power: 1 - cost_weight: 10.0 # Reduced from 18.0 to allow closer approach + cost_weight: 18.0 consider_footprint: False collision_cost: 10000.0 - collision_margin_distance: 0.05 # Reduced from 0.12 for tighter navigation - near_goal_distance: 0.4 + collision_margin_distance: 0.12 + near_goal_distance: 0.5 GoalAngleCritic: enabled: True - cost_power: 4 + cost_power: 1 cost_weight: 10.0 threshold_to_consider: 0.6 PreferForwardCritic: enabled: True cost_power: 1 - cost_weight: 16.0 + cost_weight: 3.0 threshold_to_consider: 0.5 CostCritic: # Penalizes high-cost areas @@ -304,8 +304,8 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 5.0 # Faster decay (less repulsion) - inflation_radius: 0.30 # Original value + cost_scaling_factor: 3.5 # Decay rate (lower = larger danger zone) + inflation_radius: 0.25 # Radius to inflate costs around obstacles (m) always_send_full_costmap: True @@ -348,8 +348,8 @@ global_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 6.0 # Faster decay (less repulsion) - inflation_radius: 0.32 # Original value + cost_scaling_factor: 3.5 # Decay rate + inflation_radius: 0.3 # Radius to inflate costs around obstacles (m) always_send_full_costmap: True @@ -443,11 +443,11 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 1.2] # Kinematic hard limits [vx, vy, wz] - min_velocity: [-0.35, 0.0, -1.2] - max_accel: [2.0, 0.0, 2.5] # Max acceleration [ax, ay, az] - max_decel: [-2.0, 0.0, -2.5] + max_velocity: [0.26, 0.0, 0.8] # Kinematic hard limits [vx, vy, wz] + min_velocity: [-0.26, 0.0, -0.8] + max_accel: [1.3, 0.0, 2.0] # Max acceleration [ax, ay, az] + max_decel: [-1.3, 0.0, -2.0] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file + velocity_timeout: 1.0 diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index 0aa07cc..a1f8382 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -3,8 +3,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import UnlessCondition -from launch.substitutions import EnvironmentVariable, LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression from launch_ros.actions import Node @@ -15,7 +15,8 @@ def generate_launch_description(): with open(urdf_file, "r") as infp: robot_desc = infp.read() - nav2_config_file = os.path.join(pkg_dir, "config", "nav2_params.yaml") + nav2_config_file_real = os.path.join(pkg_dir, "config", "nav2_params.yaml") + nav2_config_file_sim = os.path.join(pkg_dir, "config", "nav2_params_sim.yaml") channel_type = LaunchConfiguration( "channel_type", @@ -57,6 +58,11 @@ def generate_launch_description(): ) use_sim = LaunchConfiguration("use_sim", default="false") + # Conditionally select nav2 config file based on use_sim + nav2_config_file = PythonExpression([ + "'", nav2_config_file_sim, "' if '", use_sim, "' == 'true' else '", nav2_config_file_real, "'" + ]) + return LaunchDescription( [ DeclareLaunchArgument( diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index 0f8dc8f..558e4d8 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -4,7 +4,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import Node @@ -16,7 +16,8 @@ def generate_launch_description(): robot_desc = infp.read() slam_config_file = os.path.join(pkg_dir, "config", "slam_params.yaml") - nav2_config_file = os.path.join(pkg_dir, "config", "nav2_params.yaml") + nav2_config_file_real = os.path.join(pkg_dir, "config", "nav2_params.yaml") + nav2_config_file_sim = os.path.join(pkg_dir, "config", "nav2_params_sim.yaml") m_explorer_config_file = os.path.join( pkg_dir, "config", "m_explorer_ros2_params.yaml" ) @@ -31,6 +32,11 @@ def generate_launch_description(): map_yaml_file = LaunchConfiguration("map_yaml_file", default="") use_sim = LaunchConfiguration("use_sim", default="false") + # Conditionally select nav2 config file based on use_sim + nav2_config_file = PythonExpression([ + "'", nav2_config_file_sim, "' if '", use_sim, "' == 'true' else '", nav2_config_file_real, "'" + ]) + return LaunchDescription( [ DeclareLaunchArgument( diff --git a/m-explore-ros2 b/m-explore-ros2 new file mode 160000 index 0000000..926033f --- /dev/null +++ b/m-explore-ros2 @@ -0,0 +1 @@ +Subproject commit 926033fdef55af37ec0455660c0e4d00aa47e82f From 48b9e7ff8493f1f00c64f05abe928533ac797116 Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Thu, 11 Dec 2025 11:37:09 -0800 Subject: [PATCH 04/13] Integrate Nav2 stack with dynamic simulation parameter overrides for sim --- go2_sdk/config/nav2_params.yaml | 2 +- go2_sdk/launch/nav2_launch.py | 512 +++++++++++++++++--------------- go2_sdk/launch/slam_launch.py | 464 +++++++++++++++-------------- 3 files changed, 507 insertions(+), 471 deletions(-) diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index 04c18ac..fa8af6c 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -450,4 +450,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 + velocity_timeout: 1.0 \ No newline at end of file diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index a1f8382..c27d7b9 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -2,22 +2,285 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, TimerAction from launch.conditions import IfCondition, UnlessCondition -from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression +from launch.substitutions import EnvironmentVariable, LaunchConfiguration from launch_ros.actions import Node -def generate_launch_description(): +# Simulation-specific parameter overrides as (node_name, param_path, value) tuples +# These are applied via ros2 param set when use_sim:=true +SIM_PARAM_OVERRIDES = [ + # Controller server - MPPI controller params + ("/controller_server", "FollowPath.vx_std", "0.2"), + ("/controller_server", "FollowPath.wz_std", "0.4"), + ("/controller_server", "FollowPath.vx_max", "0.5"), + ("/controller_server", "FollowPath.vx_min", "-0.35"), + ("/controller_server", "FollowPath.wz_max", "1.2"), + ("/controller_server", "FollowPath.ObstaclesCritic.repulsion_weight", "10.0"), + ("/controller_server", "FollowPath.ObstaclesCritic.collision_margin_distance", "0.05"), + ("/controller_server", "FollowPath.ObstaclesCritic.near_goal_distance", "0.4"), + ("/controller_server", "FollowPath.GoalAngleCritic.cost_power", "4"), + ("/controller_server", "FollowPath.PreferForwardCritic.cost_weight", "16.0"), + # Local costmap inflation layer + ("/local_costmap/local_costmap", "inflation_layer.cost_scaling_factor", "5.0"), + ("/local_costmap/local_costmap", "inflation_layer.inflation_radius", "0.30"), + # Velocity smoother + ("/velocity_smoother", "max_velocity", "[0.5, 0.0, 1.2]"), + ("/velocity_smoother", "min_velocity", "[-0.35, 0.0, -1.2]"), + ("/velocity_smoother", "max_accel", "[2.0, 0.0, 2.5]"), + ("/velocity_smoother", "max_decel", "[-2.0, 0.0, -2.5]"), +] + + +def configure_nav2_nodes(context, *args, **kwargs): + """Configure Nav2 nodes with optional simulation parameter overrides.""" pkg_dir = get_package_share_directory("go2_sdk") urdf_file = os.path.join(pkg_dir, "urdf", "go2.urdf") with open(urdf_file, "r") as infp: robot_desc = infp.read() - nav2_config_file_real = os.path.join(pkg_dir, "config", "nav2_params.yaml") - nav2_config_file_sim = os.path.join(pkg_dir, "config", "nav2_params_sim.yaml") + nav2_config_file = os.path.join(pkg_dir, "config", "nav2_params.yaml") + + # Resolve use_sim at launch time + use_sim_str = LaunchConfiguration("use_sim").perform(context) + is_sim = use_sim_str.lower() == "true" + use_sim = LaunchConfiguration("use_sim") + map_yaml_file = LaunchConfiguration("map_yaml_file") + global_localization_particles = LaunchConfiguration("global_localization_particles") + + # Build ros2 param set commands for simulation overrides + sim_param_commands = [] + if is_sim: + for node_name, param_path, value in SIM_PARAM_OVERRIDES: + sim_param_commands.append( + ExecuteProcess( + cmd=["ros2", "param", "set", node_name, param_path, value], + output="screen", + ) + ) + + # Wrap sim param commands in a TimerAction to wait for nodes to be ready + sim_param_timer = TimerAction( + period=10.0, # Wait for Nav2 nodes to be fully active + actions=sim_param_commands, + ) if sim_param_commands else None + + # Standard params for all nodes + standard_params = [nav2_config_file, {"use_sim_time": use_sim}] + nodes = [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_desc}], + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher_laser", + arguments=[ + "--x", "0.2", + "--y", "0", + "--z", "0.05", + "--roll", "0", + "--pitch", "0", + "--yaw", "3.14159", + "--frame-id", "base_link", + "--child-frame-id", "laser", + ], + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="pose_to_tf", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="cmd_vel_to_go2", + name="cmd_vel_to_go2", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="go2_sport_action", + name="go2_sport_action", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="waypoint_manager", + name="waypoint_manager", + output="screen", + ), + Node( + package="go2_sdk", + executable="go2_nav2_api", + name="go2_nav2_api_node", + output="screen", + ), + Node( + package="joy", + executable="joy_node", + name="joy_node", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="teleop_twist_joy", + executable="teleop_node", + name="teleop_twist_joy_node", + output="screen", + parameters=[ + { + "axis_linear.x": 1, + "axis_linear.y": 0, + "axis_angular.z": 3, + "enable_button": 10, + "scale_linear.x": 0.5, + "scale_angular.z": 0.5, + "enable_turbo_button": 9, + "scale_turbo_linear.x": 1.5, + "scale_turbo_angular.z": 2.0, + } + ], + condition=UnlessCondition(use_sim), + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + parameters=[ + {"use_sim_time": False}, + {"autostart": True}, + { + "node_names": [ + "controller_server", + "smoother_server", + "planner_server", + "behavior_server", + "bt_navigator", + "waypoint_follower", + "velocity_smoother", + ] + }, + {"use_sim_time": use_sim}, + ], + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + parameters=standard_params, + remappings=[("/cmd_vel", "/cmd_vel")], + ), + Node( + package="nav2_smoother", + executable="smoother_server", + name="smoother_server", + output="screen", + parameters=standard_params, + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + parameters=standard_params, + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + parameters=standard_params, + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + parameters=standard_params, + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=standard_params, + ), + Node( + package="nav2_velocity_smoother", + executable="velocity_smoother", + name="velocity_smoother", + output="screen", + parameters=standard_params, + remappings=[ + ("/cmd_vel", "/cmd_vel_nav"), + ("/cmd_vel_smoothed", "/cmd_vel"), + ], + ), + Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + output="screen", + parameters=[{"use_sim_time": use_sim, "yaml_filename": map_yaml_file}], + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_localization", + output="screen", + parameters=[ + {"use_sim_time": use_sim}, + {"autostart": True}, + {"node_names": ["map_server"]}, + ], + ), + Node( + package="go2_sdk", + executable="go2_lidar_localization", + name="go2_lidar_localization", + output="screen", + parameters=[ + { + "base_frame": "base_link", + "odom_frame": "odom", + "laser_frame": "laser", + "laser_topic": "scan", + "global_localization_particles": global_localization_particles, + "use_sim_time": use_sim, + } + ], + ), + ] + + # Add the sim param timer if we're in simulation mode + if sim_param_timer: + nodes.append(sim_param_timer) + + return nodes + + +def generate_launch_description(): channel_type = LaunchConfiguration( "channel_type", default=EnvironmentVariable("LIDAR_CHANNEL_TYPE", default_value="serial"), @@ -58,11 +321,6 @@ def generate_launch_description(): ) use_sim = LaunchConfiguration("use_sim", default="false") - # Conditionally select nav2 config file based on use_sim - nav2_config_file = PythonExpression([ - "'", nav2_config_file_sim, "' if '", use_sim, "' == 'true' else '", nav2_config_file_real, "'" - ]) - return LaunchDescription( [ DeclareLaunchArgument( @@ -114,238 +372,6 @@ def generate_launch_description(): default_value=use_sim, description="Whether to use simulation", ), - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[{"robot_description": robot_desc}], - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="joint_state_publisher", - name="joint_state_publisher", - output="screen", - condition=UnlessCondition(use_sim), - ), - # Node( - # package='rplidar_ros', - # executable='rplidar_node', - # name='rplidar_node', - # parameters=[{ - # 'channel_type': channel_type, - # 'serial_port': serial_port, - # 'serial_baudrate': serial_baudrate, - # 'frame_id': frame_id, - # 'inverted': inverted, - # 'angle_compensate': angle_compensate - # }], - # output='screen'), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher_laser", - arguments=[ - "--x", - "0.2", - "--y", - "0", - "--z", - "0.05", - "--roll", - "0", - "--pitch", - "0", - "--yaw", - "3.14159", - "--frame-id", - "base_link", - "--child-frame-id", - "laser", - ], - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="pose_to_tf", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="cmd_vel_to_go2", - name="cmd_vel_to_go2", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="go2_sport_action", - name="go2_sport_action", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="waypoint_manager", - name="waypoint_manager", - output="screen", - ), - Node( - package="go2_sdk", - executable="go2_nav2_api", - name="go2_nav2_api_node", - output="screen", - ), - Node( - package="joy", - executable="joy_node", - name="joy_node", - output="screen", - condition=UnlessCondition(use_sim), - ), - # RB is the enable button - # The left joystick controls linear movement - # The right joystick controls angular movement - Node( - package="teleop_twist_joy", - executable="teleop_node", - name="teleop_twist_joy_node", - output="screen", - parameters=[ - { - "axis_linear.x": 1, - "axis_linear.y": 0, - "axis_angular.z": 3, - "enable_button": 10, - "scale_linear.x": 0.5, - "scale_angular.z": 0.5, - "enable_turbo_button": 9, - "scale_turbo_linear.x": 1.5, - "scale_turbo_angular.z": 2.0, - } - ], - condition=UnlessCondition(use_sim), - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager_navigation", - output="screen", - parameters=[ - {"use_sim_time": False}, - {"autostart": True}, - { - "node_names": [ - "controller_server", - "smoother_server", - "planner_server", - "behavior_server", - "bt_navigator", - "waypoint_follower", - "velocity_smoother", - ] - }, - {"use_sim_time": use_sim}, - ], - ), - Node( - package="nav2_controller", - executable="controller_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - remappings=[("/cmd_vel", "/cmd_vel")], - ), - Node( - package="nav2_smoother", - executable="smoother_server", - name="smoother_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_planner", - executable="planner_server", - name="planner_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_behaviors", - executable="behavior_server", - name="behavior_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_bt_navigator", - executable="bt_navigator", - name="bt_navigator", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_waypoint_follower", - executable="waypoint_follower", - name="waypoint_follower", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_velocity_smoother", - executable="velocity_smoother", - name="velocity_smoother", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - remappings=[ - ("/cmd_vel", "/cmd_vel_nav"), - ("/cmd_vel_smoothed", "/cmd_vel"), - ], - ), - Node( - package="nav2_map_server", - executable="map_server", - name="map_server", - output="screen", - parameters=[{"use_sim_time": use_sim, "yaml_filename": map_yaml_file}], - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager_localization", - output="screen", - parameters=[ - {"use_sim_time": use_sim}, - {"autostart": True}, - # {'node_names': ['map_server', 'amcl']} - {"node_names": ["map_server"]}, - ], - ), - # Node( - # package='nav2_amcl', - # executable='amcl', - # name='amcl', - # output='screen', - # parameters=[nav2_config_file] - # ) - # Custom Lidar Localization Node - Node( - package="go2_sdk", - executable="go2_lidar_localization", - name="go2_lidar_localization", - output="screen", - parameters=[ - { - "base_frame": "base_link", - "odom_frame": "odom", - "laser_frame": "laser", - "laser_topic": "scan", - "global_localization_particles": global_localization_particles, - "use_sim_time": use_sim, - } - ], - ), + OpaqueFunction(function=configure_nav2_nodes), ] ) diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index 558e4d8..60c3503 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -2,13 +2,38 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, TimerAction +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -def generate_launch_description(): +# Simulation-specific parameter overrides as (node_name, param_path, value) tuples +# These are applied via ros2 param set when use_sim:=true +SIM_PARAM_OVERRIDES = [ + # Controller server - MPPI controller params + ("/controller_server", "FollowPath.vx_std", "0.2"), + ("/controller_server", "FollowPath.wz_std", "0.4"), + ("/controller_server", "FollowPath.vx_max", "0.5"), + ("/controller_server", "FollowPath.vx_min", "-0.35"), + ("/controller_server", "FollowPath.wz_max", "1.2"), + ("/controller_server", "FollowPath.ObstaclesCritic.repulsion_weight", "10.0"), + ("/controller_server", "FollowPath.ObstaclesCritic.collision_margin_distance", "0.05"), + ("/controller_server", "FollowPath.ObstaclesCritic.near_goal_distance", "0.4"), + ("/controller_server", "FollowPath.GoalAngleCritic.cost_power", "4"), + ("/controller_server", "FollowPath.PreferForwardCritic.cost_weight", "16.0"), + # Local costmap inflation layer + ("/local_costmap/local_costmap", "inflation_layer.cost_scaling_factor", "5.0"), + ("/local_costmap/local_costmap", "inflation_layer.inflation_radius", "0.30"), + # Velocity smoother + ("/velocity_smoother", "max_velocity", "[0.5, 0.0, 1.2]"), + ("/velocity_smoother", "min_velocity", "[-0.35, 0.0, -1.2]"), + ("/velocity_smoother", "max_accel", "[2.0, 0.0, 2.5]"), + ("/velocity_smoother", "max_decel", "[-2.0, 0.0, -2.5]"), +] + +def configure_slam_nodes(context, *args, **kwargs): + """Configure SLAM and navigation nodes with optional simulation overrides.""" pkg_dir = get_package_share_directory("go2_sdk") urdf_file = os.path.join(pkg_dir, "urdf", "go2.urdf") @@ -16,12 +41,217 @@ def generate_launch_description(): robot_desc = infp.read() slam_config_file = os.path.join(pkg_dir, "config", "slam_params.yaml") - nav2_config_file_real = os.path.join(pkg_dir, "config", "nav2_params.yaml") - nav2_config_file_sim = os.path.join(pkg_dir, "config", "nav2_params_sim.yaml") + nav2_config_file = os.path.join(pkg_dir, "config", "nav2_params.yaml") m_explorer_config_file = os.path.join( pkg_dir, "config", "m_explorer_ros2_params.yaml" ) + # Resolve use_sim at launch time + use_sim_str = LaunchConfiguration("use_sim").perform(context) + is_sim = use_sim_str.lower() == "true" + use_sim = LaunchConfiguration("use_sim") + + # Build ros2 param set commands for simulation overrides + sim_param_commands = [] + if is_sim: + for node_name, param_path, value in SIM_PARAM_OVERRIDES: + sim_param_commands.append( + ExecuteProcess( + cmd=["ros2", "param", "set", node_name, param_path, value], + output="screen", + ) + ) + + # Wrap sim param commands in a TimerAction to wait for nodes to be ready + sim_param_timer = TimerAction( + period=10.0, # Wait for Nav2 nodes to be fully active + actions=sim_param_commands, + ) if sim_param_commands else None + + nodes = [ + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_desc}], + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher_laser", + arguments=[ + "--x", "0.2", + "--y", "0", + "--z", "0.05", + "--roll", "0", + "--pitch", "0", + "--yaw", "3.14159", + "--frame-id", "base_link", + "--child-frame-id", "laser", + ], + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="pose_to_tf", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="cmd_vel_to_go2", + name="cmd_vel_to_go2", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="go2_sport_action", + name="go2_sport_action", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="waypoint_manager", + name="waypoint_manager", + output="screen", + ), + Node( + package="joy", + executable="joy_node", + name="joy_node", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="teleop_twist_joy", + executable="teleop_node", + name="teleop_twist_joy_node", + output="screen", + parameters=[ + { + "axis_linear.x": 1, + "axis_linear.y": 0, + "axis_angular.z": 3, + "enable_button": 10, + "scale_linear.x": 0.5, + "scale_angular.z": 1.0, + "enable_turbo_button": 9, + "scale_turbo_linear.x": 1.5, + "scale_turbo_angular.z": 2.0, + } + ], + condition=UnlessCondition(use_sim), + ), + Node( + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + parameters=[slam_config_file, {"use_sim_time": use_sim}], + output="screen", + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + parameters=[ + {"use_sim_time": use_sim}, + {"autostart": True}, + { + "node_names": [ + "controller_server", + "smoother_server", + "planner_server", + "behavior_server", + "bt_navigator", + "waypoint_follower", + "velocity_smoother", + ] + }, + ], + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + remappings=[("/cmd_vel", "/cmd_vel")], + ), + Node( + package="nav2_smoother", + executable="smoother_server", + name="smoother_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_velocity_smoother", + executable="velocity_smoother", + name="velocity_smoother", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + remappings=[ + ("/cmd_vel", "/cmd_vel_nav"), + ("/cmd_vel_smoothed", "/cmd_vel"), + ], + ), + Node( + package="frontier_explorer", + executable="explore", + name="frontier_explorer", + output="screen", + parameters=[m_explorer_config_file, {"use_sim_time": use_sim}], + ), + ] + + # Add the sim param timer if we're in simulation mode + if sim_param_timer: + nodes.append(sim_param_timer) + + return nodes + + +def generate_launch_description(): channel_type = LaunchConfiguration("channel_type", default="serial") serial_port = LaunchConfiguration("serial_port", default="/dev/ttyUSB0") serial_baudrate = LaunchConfiguration("serial_baudrate", default="115200") @@ -32,11 +262,6 @@ def generate_launch_description(): map_yaml_file = LaunchConfiguration("map_yaml_file", default="") use_sim = LaunchConfiguration("use_sim", default="false") - # Conditionally select nav2 config file based on use_sim - nav2_config_file = PythonExpression([ - "'", nav2_config_file_sim, "' if '", use_sim, "' == 'true' else '", nav2_config_file_real, "'" - ]) - return LaunchDescription( [ DeclareLaunchArgument( @@ -84,221 +309,6 @@ def generate_launch_description(): default_value=use_sim, description="Whether to use simulation", ), - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[{"robot_description": robot_desc}], - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="joint_state_publisher", - name="joint_state_publisher", - output="screen", - condition=UnlessCondition(use_sim), - ), - # Node( - # package='rplidar_ros', - # executable='rplidar_node', - # name='rplidar_node', - # parameters=[{ - # 'channel_type': channel_type, - # 'serial_port': serial_port, - # 'serial_baudrate': serial_baudrate, - # 'frame_id': frame_id, - # 'inverted': inverted, - # 'angle_compensate': angle_compensate - # }], - # output='screen'), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher_laser", - arguments=[ - "--x", - "0.2", - "--y", - "0", - "--z", - "0.05", - "--roll", - "0", - "--pitch", - "0", - "--yaw", - "3.14159", - "--frame-id", - "base_link", - "--child-frame-id", - "laser", - ], - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="pose_to_tf", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="cmd_vel_to_go2", - name="cmd_vel_to_go2", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="go2_sport_action", - name="go2_sport_action", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="waypoint_manager", - name="waypoint_manager", - output="screen", - ), - Node( - package="joy", - executable="joy_node", - name="joy_node", - output="screen", - condition=UnlessCondition(use_sim), - ), - # RB is the enable button - # The left joystick controls linear movement - # The right joystick controls angular movement - Node( - package="teleop_twist_joy", - executable="teleop_node", - name="teleop_twist_joy_node", - output="screen", - parameters=[ - { - "axis_linear.x": 1, - "axis_linear.y": 0, - "axis_angular.z": 3, - "enable_button": 10, - "scale_linear.x": 0.5, - "scale_angular.z": 1.0, - "enable_turbo_button": 9, - "scale_turbo_linear.x": 1.5, - "scale_turbo_angular.z": 2.0, - } - ], - condition=UnlessCondition(use_sim), - ), - Node( - package="slam_toolbox", - executable="sync_slam_toolbox_node", - name="slam_toolbox", - parameters=[slam_config_file, {"use_sim_time": use_sim}], - output="screen", - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager_navigation", - output="screen", - parameters=[ - {"use_sim_time": use_sim}, - {"autostart": True}, - { - "node_names": [ - "controller_server", - "smoother_server", - "planner_server", - "behavior_server", - "bt_navigator", - "waypoint_follower", - "velocity_smoother", - ] - }, - ], - ), - Node( - package="nav2_controller", - executable="controller_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - remappings=[ - ("/cmd_vel", "/cmd_vel"), - ], - ), - Node( - package="nav2_smoother", - executable="smoother_server", - name="smoother_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_planner", - executable="planner_server", - name="planner_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_behaviors", - executable="behavior_server", - name="behavior_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_bt_navigator", - executable="bt_navigator", - name="bt_navigator", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_waypoint_follower", - executable="waypoint_follower", - name="waypoint_follower", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_velocity_smoother", - executable="velocity_smoother", - name="velocity_smoother", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - remappings=[ - ("/cmd_vel", "/cmd_vel_nav"), - ("/cmd_vel_smoothed", "/cmd_vel"), - ], - ), - # Node( - # package='nav2_lifecycle_manager', - # executable='lifecycle_manager', - # name='lifecycle_manager_localization', - # output='screen', - # parameters=[ - # {'use_sim_time': False}, - # {'autostart': True}, - # {'node_names': ['amcl']}] - # ), - # Node( - # package='nav2_amcl', - # executable='amcl', - # name='amcl', - # output='screen', - # parameters=[nav2_config_file] - # ) - Node( - package="frontier_explorer", - executable="explore", - name="frontier_explorer", - output="screen", - parameters=[m_explorer_config_file, {"use_sim_time": use_sim}], - ), + OpaqueFunction(function=configure_slam_nodes), ] ) From a1aee62755425ba722a9e1b7e3eeff80a59b31cc Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Thu, 11 Dec 2025 11:52:07 -0800 Subject: [PATCH 05/13] Lint Fixes --- .../go2_description/worlds/maze_world.sdf | 2 +- .../go2_gazebo_sim/launch/go2_launch.py | 9 +-- go2_sdk/config/nav2_params.yaml | 30 +++++----- go2_sdk/config/nav2_params_sim.yaml | 30 +++++----- go2_sdk/launch/nav2_launch.py | 56 ++++++++++++------- go2_sdk/launch/slam_launch.py | 53 ++++++++++++------ 6 files changed, 109 insertions(+), 71 deletions(-) diff --git a/go2_gazebo_sim/go2_description/worlds/maze_world.sdf b/go2_gazebo_sim/go2_description/worlds/maze_world.sdf index 51f8cb6..eb4ffdc 100644 --- a/go2_gazebo_sim/go2_description/worlds/maze_world.sdf +++ b/go2_gazebo_sim/go2_description/worlds/maze_world.sdf @@ -670,4 +670,4 @@ - \ No newline at end of file + diff --git a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py index 79be63e..ee931c1 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py +++ b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py @@ -7,8 +7,8 @@ DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, - TimerAction, SetEnvironmentVariable, + TimerAction, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -42,15 +42,16 @@ def generate_launch_description(): current_gz_resource_path = os.environ.get("GZ_SIM_RESOURCE_PATH", "") if go2_description_models not in current_gz_resource_path: if current_gz_resource_path: - new_gz_resource_path = current_gz_resource_path + ":" + go2_description_models + new_gz_resource_path = ( + current_gz_resource_path + ":" + go2_description_models + ) else: new_gz_resource_path = go2_description_models else: new_gz_resource_path = current_gz_resource_path set_gz_resource_path = SetEnvironmentVariable( - name="GZ_SIM_RESOURCE_PATH", - value=new_gz_resource_path + name="GZ_SIM_RESOURCE_PATH", value=new_gz_resource_path ) declare_use_sim_time = DeclareLaunchArgument( diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index fa8af6c..936673c 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -30,7 +30,7 @@ amcl: save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: True - transform_tolerance: 1.0 + transform_tolerance: 1.0 update_min_a: 0.005 update_min_d: 0.02 z_hit: 0.85 @@ -48,7 +48,7 @@ bt_navigator: ros__parameters: use_sim_time: False global_frame: map - transform_tolerance: 1.0 + transform_tolerance: 1.0 robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 @@ -141,7 +141,7 @@ controller_server: wz_max: 0.8 # Max angular velocity (rad/s) iteration_count: 1 # Optimization iterations (1 is efficient) prune_distance: 1.5 # Remove path points closer than this - transform_tolerance: 0.5 + transform_tolerance: 0.5 temperature: 0.4 gamma: 0.015 motion_model: "DiffDrive" @@ -239,7 +239,7 @@ local_costmap: width: 4 height: 4 resolution: 0.05 - transform_tolerance: 1.0 + transform_tolerance: 1.0 # Slightly smaller footprint to prevent self-collision in costmap footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" # Robot shape [x,y] for collision checking plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] @@ -264,7 +264,7 @@ local_costmap: raytrace_max_range: 3.5 raytrace_min_range: 0.0 obstacle_max_range: 3.0 - obstacle_min_range: 0.15 + obstacle_min_range: 0.15 stvl_layer: plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" @@ -272,15 +272,15 @@ local_costmap: voxel_decay: 1.5 # Faster decay decay_model: 0 voxel_size: 0.05 - track_unknown_space: False + track_unknown_space: False observation_persistence: 0.0 max_obstacle_height: 2.5 min_obstacle_height: 0.18 # Slightly higher to avoid ground noise obstacle_max_range: 2.5 - obstacle_min_range: 0.25 + obstacle_min_range: 0.25 origin_z: 0.0 publish_voxel_map: True - transform_tolerance: 1.0 + transform_tolerance: 1.0 mapping_mode: False map_save_duration: 60.0 combination_method: 1 @@ -297,7 +297,7 @@ local_costmap: observation_persistence: 0.0 inf_is_valid: False filter: "voxel" - voxel_min_points: 3 + voxel_min_points: 3 clear_after_reading: True enabled: True model_type: 1 @@ -317,17 +317,17 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: False - transform_tolerance: 1.0 + transform_tolerance: 1.0 # Slightly smaller footprint footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" resolution: 0.1 - track_unknown_space: False + track_unknown_space: False plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - footprint_clearing_enabled: True + footprint_clearing_enabled: True max_obstacle_height: 2.0 observation_sources: scan scan: @@ -369,12 +369,12 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 5.0 - transform_tolerance: 1.0 + transform_tolerance: 1.0 use_sim_time: False planner_plugins: ["GridBased"] GridBased: plugin: "nav2_smac_planner/SmacPlanner2D" - tolerance: 0.5 + tolerance: 0.5 downsample_costmap: False downsampling_factor: 1 allow_unknown: True # Allow planning through unknown space @@ -450,4 +450,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file + velocity_timeout: 1.0 diff --git a/go2_sdk/config/nav2_params_sim.yaml b/go2_sdk/config/nav2_params_sim.yaml index 6f9c669..6e2294d 100644 --- a/go2_sdk/config/nav2_params_sim.yaml +++ b/go2_sdk/config/nav2_params_sim.yaml @@ -30,7 +30,7 @@ amcl: save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: True - transform_tolerance: 1.0 + transform_tolerance: 1.0 update_min_a: 0.005 update_min_d: 0.02 z_hit: 0.85 @@ -48,7 +48,7 @@ bt_navigator: ros__parameters: use_sim_time: False global_frame: map - transform_tolerance: 1.0 + transform_tolerance: 1.0 robot_base_frame: base_link odom_topic: /odom bt_loop_duration: 10 @@ -141,7 +141,7 @@ controller_server: wz_max: 1.2 # Max angular velocity (rad/s) iteration_count: 1 # Optimization iterations (1 is efficient) prune_distance: 1.5 # Remove path points closer than this - transform_tolerance: 0.5 + transform_tolerance: 0.5 temperature: 0.4 gamma: 0.015 motion_model: "DiffDrive" @@ -239,7 +239,7 @@ local_costmap: width: 4 height: 4 resolution: 0.05 - transform_tolerance: 1.0 + transform_tolerance: 1.0 # Slightly smaller footprint to prevent self-collision in costmap footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" # Robot shape [x,y] for collision checking plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] @@ -264,7 +264,7 @@ local_costmap: raytrace_max_range: 3.5 raytrace_min_range: 0.0 obstacle_max_range: 3.0 - obstacle_min_range: 0.15 + obstacle_min_range: 0.15 stvl_layer: plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" @@ -272,15 +272,15 @@ local_costmap: voxel_decay: 1.5 # Faster decay decay_model: 0 voxel_size: 0.05 - track_unknown_space: False + track_unknown_space: False observation_persistence: 0.0 max_obstacle_height: 2.5 min_obstacle_height: 0.18 # Slightly higher to avoid ground noise obstacle_max_range: 2.5 - obstacle_min_range: 0.25 + obstacle_min_range: 0.25 origin_z: 0.0 publish_voxel_map: True - transform_tolerance: 1.0 + transform_tolerance: 1.0 mapping_mode: False map_save_duration: 60.0 combination_method: 1 @@ -297,7 +297,7 @@ local_costmap: observation_persistence: 0.0 inf_is_valid: False filter: "voxel" - voxel_min_points: 3 + voxel_min_points: 3 clear_after_reading: True enabled: True model_type: 1 @@ -317,17 +317,17 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: False - transform_tolerance: 1.0 + transform_tolerance: 1.0 # Slightly smaller footprint footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" resolution: 0.1 - track_unknown_space: False + track_unknown_space: False plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True - footprint_clearing_enabled: True + footprint_clearing_enabled: True max_obstacle_height: 2.0 observation_sources: scan scan: @@ -369,12 +369,12 @@ map_saver: planner_server: ros__parameters: expected_planner_frequency: 5.0 - transform_tolerance: 1.0 + transform_tolerance: 1.0 use_sim_time: False planner_plugins: ["GridBased"] GridBased: plugin: "nav2_smac_planner/SmacPlanner2D" - tolerance: 0.5 + tolerance: 0.5 downsample_costmap: False downsampling_factor: 1 allow_unknown: True # Allow planning through unknown space @@ -450,4 +450,4 @@ velocity_smoother: odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file + velocity_timeout: 1.0 diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index c27d7b9..074021e 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -2,12 +2,16 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, TimerAction -from launch.conditions import IfCondition, UnlessCondition +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + OpaqueFunction, + TimerAction, +) +from launch.conditions import UnlessCondition from launch.substitutions import EnvironmentVariable, LaunchConfiguration from launch_ros.actions import Node - # Simulation-specific parameter overrides as (node_name, param_path, value) tuples # These are applied via ros2 param set when use_sim:=true SIM_PARAM_OVERRIDES = [ @@ -18,7 +22,11 @@ ("/controller_server", "FollowPath.vx_min", "-0.35"), ("/controller_server", "FollowPath.wz_max", "1.2"), ("/controller_server", "FollowPath.ObstaclesCritic.repulsion_weight", "10.0"), - ("/controller_server", "FollowPath.ObstaclesCritic.collision_margin_distance", "0.05"), + ( + "/controller_server", + "FollowPath.ObstaclesCritic.collision_margin_distance", + "0.05", + ), ("/controller_server", "FollowPath.ObstaclesCritic.near_goal_distance", "0.4"), ("/controller_server", "FollowPath.GoalAngleCritic.cost_power", "4"), ("/controller_server", "FollowPath.PreferForwardCritic.cost_weight", "16.0"), @@ -62,10 +70,14 @@ def configure_nav2_nodes(context, *args, **kwargs): ) # Wrap sim param commands in a TimerAction to wait for nodes to be ready - sim_param_timer = TimerAction( - period=10.0, # Wait for Nav2 nodes to be fully active - actions=sim_param_commands, - ) if sim_param_commands else None + sim_param_timer = ( + TimerAction( + period=10.0, # Wait for Nav2 nodes to be fully active + actions=sim_param_commands, + ) + if sim_param_commands + else None + ) # Standard params for all nodes standard_params = [nav2_config_file, {"use_sim_time": use_sim}] @@ -91,14 +103,22 @@ def configure_nav2_nodes(context, *args, **kwargs): executable="static_transform_publisher", name="static_transform_publisher_laser", arguments=[ - "--x", "0.2", - "--y", "0", - "--z", "0.05", - "--roll", "0", - "--pitch", "0", - "--yaw", "3.14159", - "--frame-id", "base_link", - "--child-frame-id", "laser", + "--x", + "0.2", + "--y", + "0", + "--z", + "0.05", + "--roll", + "0", + "--pitch", + "0", + "--yaw", + "3.14159", + "--frame-id", + "base_link", + "--child-frame-id", + "laser", ], output="screen", condition=UnlessCondition(use_sim), @@ -309,10 +329,6 @@ def generate_launch_description(): "scan_mode", default=EnvironmentVariable("LIDAR_SCAN_MODE", default_value="Sensitivity"), ) - map_yaml_file = LaunchConfiguration( - "map_yaml_file", - default=EnvironmentVariable("MAP_YAML_FILE", default_value=""), - ) global_localization_particles = LaunchConfiguration( "global_localization_particles", default=EnvironmentVariable( diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index 60c3503..2687ffa 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -2,12 +2,16 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, TimerAction -from launch.conditions import IfCondition, UnlessCondition +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + OpaqueFunction, + TimerAction, +) +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node - # Simulation-specific parameter overrides as (node_name, param_path, value) tuples # These are applied via ros2 param set when use_sim:=true SIM_PARAM_OVERRIDES = [ @@ -18,7 +22,11 @@ ("/controller_server", "FollowPath.vx_min", "-0.35"), ("/controller_server", "FollowPath.wz_max", "1.2"), ("/controller_server", "FollowPath.ObstaclesCritic.repulsion_weight", "10.0"), - ("/controller_server", "FollowPath.ObstaclesCritic.collision_margin_distance", "0.05"), + ( + "/controller_server", + "FollowPath.ObstaclesCritic.collision_margin_distance", + "0.05", + ), ("/controller_server", "FollowPath.ObstaclesCritic.near_goal_distance", "0.4"), ("/controller_server", "FollowPath.GoalAngleCritic.cost_power", "4"), ("/controller_server", "FollowPath.PreferForwardCritic.cost_weight", "16.0"), @@ -32,6 +40,7 @@ ("/velocity_smoother", "max_decel", "[-2.0, 0.0, -2.5]"), ] + def configure_slam_nodes(context, *args, **kwargs): """Configure SLAM and navigation nodes with optional simulation overrides.""" pkg_dir = get_package_share_directory("go2_sdk") @@ -63,10 +72,14 @@ def configure_slam_nodes(context, *args, **kwargs): ) # Wrap sim param commands in a TimerAction to wait for nodes to be ready - sim_param_timer = TimerAction( - period=10.0, # Wait for Nav2 nodes to be fully active - actions=sim_param_commands, - ) if sim_param_commands else None + sim_param_timer = ( + TimerAction( + period=10.0, # Wait for Nav2 nodes to be fully active + actions=sim_param_commands, + ) + if sim_param_commands + else None + ) nodes = [ Node( @@ -89,14 +102,22 @@ def configure_slam_nodes(context, *args, **kwargs): executable="static_transform_publisher", name="static_transform_publisher_laser", arguments=[ - "--x", "0.2", - "--y", "0", - "--z", "0.05", - "--roll", "0", - "--pitch", "0", - "--yaw", "3.14159", - "--frame-id", "base_link", - "--child-frame-id", "laser", + "--x", + "0.2", + "--y", + "0", + "--z", + "0.05", + "--roll", + "0", + "--pitch", + "0", + "--yaw", + "3.14159", + "--frame-id", + "base_link", + "--child-frame-id", + "laser", ], output="screen", condition=UnlessCondition(use_sim), From 8653887c98ae70c1d685142ace5a7fb6d2fa0e4b Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 11:02:08 -0800 Subject: [PATCH 06/13] Add global costmap inflation parameters to navigation and SLAM launch files for sim --- go2_sdk/launch/nav2_launch.py | 5 ++++- go2_sdk/launch/slam_launch.py | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index 074021e..c5970a0 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -21,7 +21,7 @@ ("/controller_server", "FollowPath.vx_max", "0.5"), ("/controller_server", "FollowPath.vx_min", "-0.35"), ("/controller_server", "FollowPath.wz_max", "1.2"), - ("/controller_server", "FollowPath.ObstaclesCritic.repulsion_weight", "10.0"), + ("/controller_server", "FollowPath.ObstaclesCritic.cost_weight", "10.0"), ( "/controller_server", "FollowPath.ObstaclesCritic.collision_margin_distance", @@ -33,6 +33,9 @@ # Local costmap inflation layer ("/local_costmap/local_costmap", "inflation_layer.cost_scaling_factor", "5.0"), ("/local_costmap/local_costmap", "inflation_layer.inflation_radius", "0.30"), + # Global costmap inflation layer + ("/global_costmap/global_costmap", "inflation_layer.cost_scaling_factor", "6.0"), + ("/global_costmap/global_costmap", "inflation_layer.inflation_radius", "0.32"), # Velocity smoother ("/velocity_smoother", "max_velocity", "[0.5, 0.0, 1.2]"), ("/velocity_smoother", "min_velocity", "[-0.35, 0.0, -1.2]"), diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index 2687ffa..6d331a1 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -21,7 +21,7 @@ ("/controller_server", "FollowPath.vx_max", "0.5"), ("/controller_server", "FollowPath.vx_min", "-0.35"), ("/controller_server", "FollowPath.wz_max", "1.2"), - ("/controller_server", "FollowPath.ObstaclesCritic.repulsion_weight", "10.0"), + ("/controller_server", "FollowPath.ObstaclesCritic.cost_weight", "10.0"), ( "/controller_server", "FollowPath.ObstaclesCritic.collision_margin_distance", @@ -33,6 +33,9 @@ # Local costmap inflation layer ("/local_costmap/local_costmap", "inflation_layer.cost_scaling_factor", "5.0"), ("/local_costmap/local_costmap", "inflation_layer.inflation_radius", "0.30"), + # Global costmap inflation layer + ("/global_costmap/global_costmap", "inflation_layer.cost_scaling_factor", "6.0"), + ("/global_costmap/global_costmap", "inflation_layer.inflation_radius", "0.32"), # Velocity smoother ("/velocity_smoother", "max_velocity", "[0.5, 0.0, 1.2]"), ("/velocity_smoother", "min_velocity", "[-0.35, 0.0, -1.2]"), From 45d69b1036b1afec30246d33c89838f0eb94af06 Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 11:20:58 -0800 Subject: [PATCH 07/13] fix: remove orphaned m-explore-ros2 submodule reference --- m-explore-ros2 | 1 - 1 file changed, 1 deletion(-) delete mode 160000 m-explore-ros2 diff --git a/m-explore-ros2 b/m-explore-ros2 deleted file mode 160000 index 926033f..0000000 --- a/m-explore-ros2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 926033fdef55af37ec0455660c0e4d00aa47e82f From 97431d43d7eae944d1697fd1ef5bfaf208f70758 Mon Sep 17 00:00:00 2001 From: openminddev <147775420+openminddev@users.noreply.github.com> Date: Fri, 12 Dec 2025 13:12:06 -0800 Subject: [PATCH 08/13] Param override (#113) * Override param * Improve parameters overwritten --- go2_sdk/launch/nav2_launch.py | 569 +++++++++++++++++----------------- go2_sdk/launch/slam_launch.py | 518 ++++++++++++++++--------------- 2 files changed, 541 insertions(+), 546 deletions(-) diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index c5970a0..064a376 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -2,50 +2,53 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - OpaqueFunction, - TimerAction, -) +from launch.actions import DeclareLaunchArgument from launch.conditions import UnlessCondition from launch.substitutions import EnvironmentVariable, LaunchConfiguration from launch_ros.actions import Node -# Simulation-specific parameter overrides as (node_name, param_path, value) tuples -# These are applied via ros2 param set when use_sim:=true -SIM_PARAM_OVERRIDES = [ - # Controller server - MPPI controller params - ("/controller_server", "FollowPath.vx_std", "0.2"), - ("/controller_server", "FollowPath.wz_std", "0.4"), - ("/controller_server", "FollowPath.vx_max", "0.5"), - ("/controller_server", "FollowPath.vx_min", "-0.35"), - ("/controller_server", "FollowPath.wz_max", "1.2"), - ("/controller_server", "FollowPath.ObstaclesCritic.cost_weight", "10.0"), - ( - "/controller_server", - "FollowPath.ObstaclesCritic.collision_margin_distance", - "0.05", - ), - ("/controller_server", "FollowPath.ObstaclesCritic.near_goal_distance", "0.4"), - ("/controller_server", "FollowPath.GoalAngleCritic.cost_power", "4"), - ("/controller_server", "FollowPath.PreferForwardCritic.cost_weight", "16.0"), - # Local costmap inflation layer - ("/local_costmap/local_costmap", "inflation_layer.cost_scaling_factor", "5.0"), - ("/local_costmap/local_costmap", "inflation_layer.inflation_radius", "0.30"), - # Global costmap inflation layer - ("/global_costmap/global_costmap", "inflation_layer.cost_scaling_factor", "6.0"), - ("/global_costmap/global_costmap", "inflation_layer.inflation_radius", "0.32"), - # Velocity smoother - ("/velocity_smoother", "max_velocity", "[0.5, 0.0, 1.2]"), - ("/velocity_smoother", "min_velocity", "[-0.35, 0.0, -1.2]"), - ("/velocity_smoother", "max_accel", "[2.0, 0.0, 2.5]"), - ("/velocity_smoother", "max_decel", "[-2.0, 0.0, -2.5]"), -] +SIM_PARAM_OVERRIDES = { + "controller_server": { + "FollowPath.vx_std": 0.2, + "FollowPath.wz_std": 0.4, + "FollowPath.vx_max": 0.5, + "FollowPath.vx_min": -0.35, + "FollowPath.wz_max": 1.2, + "FollowPath.ObstaclesCritic.cost_weight": 10.0, + "FollowPath.ObstaclesCritic.collision_margin_distance": 0.05, + "FollowPath.ObstaclesCritic.near_goal_distance": 0.4, + "FollowPath.GoalAngleCritic.cost_power": 4, + "FollowPath.PreferForwardCritic.cost_weight": 16.0, + # local costmap params + "inflation_layer.cost_scaling_factor": 5.0, + "inflation_layer.inflation_radius": 0.30, + }, + "planner_server": { + # globl costmap params + "inflation_layer.cost_scaling_factor": 6.0, + "inflation_layer.inflation_radius": 0.32, + }, + "velocity_smoother": { + "max_velocity": [0.5, 0.0, 1.2], + "min_velocity": [-0.35, 0.0, -1.2], + "max_accel": [2.0, 0.0, 2.5], + "max_decel": [-2.0, 0.0, -2.5], + }, +} -def configure_nav2_nodes(context, *args, **kwargs): - """Configure Nav2 nodes with optional simulation parameter overrides.""" +def get_node_params(node_name: str, base_config: str, use_sim: bool): + """ + Merge base config with simulation overrides if needed. + """ + params = [base_config, {"use_sim_time": use_sim}] + + if use_sim and node_name in SIM_PARAM_OVERRIDES: + params.append(SIM_PARAM_OVERRIDES[node_name]) + + return params + +def generate_launch_description(): pkg_dir = get_package_share_directory("go2_sdk") urdf_file = os.path.join(pkg_dir, "urdf", "go2.urdf") @@ -54,256 +57,6 @@ def configure_nav2_nodes(context, *args, **kwargs): nav2_config_file = os.path.join(pkg_dir, "config", "nav2_params.yaml") - # Resolve use_sim at launch time - use_sim_str = LaunchConfiguration("use_sim").perform(context) - is_sim = use_sim_str.lower() == "true" - use_sim = LaunchConfiguration("use_sim") - map_yaml_file = LaunchConfiguration("map_yaml_file") - global_localization_particles = LaunchConfiguration("global_localization_particles") - - # Build ros2 param set commands for simulation overrides - sim_param_commands = [] - if is_sim: - for node_name, param_path, value in SIM_PARAM_OVERRIDES: - sim_param_commands.append( - ExecuteProcess( - cmd=["ros2", "param", "set", node_name, param_path, value], - output="screen", - ) - ) - - # Wrap sim param commands in a TimerAction to wait for nodes to be ready - sim_param_timer = ( - TimerAction( - period=10.0, # Wait for Nav2 nodes to be fully active - actions=sim_param_commands, - ) - if sim_param_commands - else None - ) - - # Standard params for all nodes - standard_params = [nav2_config_file, {"use_sim_time": use_sim}] - - nodes = [ - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[{"robot_description": robot_desc}], - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="joint_state_publisher", - name="joint_state_publisher", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher_laser", - arguments=[ - "--x", - "0.2", - "--y", - "0", - "--z", - "0.05", - "--roll", - "0", - "--pitch", - "0", - "--yaw", - "3.14159", - "--frame-id", - "base_link", - "--child-frame-id", - "laser", - ], - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="pose_to_tf", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="cmd_vel_to_go2", - name="cmd_vel_to_go2", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="go2_sport_action", - name="go2_sport_action", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="waypoint_manager", - name="waypoint_manager", - output="screen", - ), - Node( - package="go2_sdk", - executable="go2_nav2_api", - name="go2_nav2_api_node", - output="screen", - ), - Node( - package="joy", - executable="joy_node", - name="joy_node", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="teleop_twist_joy", - executable="teleop_node", - name="teleop_twist_joy_node", - output="screen", - parameters=[ - { - "axis_linear.x": 1, - "axis_linear.y": 0, - "axis_angular.z": 3, - "enable_button": 10, - "scale_linear.x": 0.5, - "scale_angular.z": 0.5, - "enable_turbo_button": 9, - "scale_turbo_linear.x": 1.5, - "scale_turbo_angular.z": 2.0, - } - ], - condition=UnlessCondition(use_sim), - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager_navigation", - output="screen", - parameters=[ - {"use_sim_time": False}, - {"autostart": True}, - { - "node_names": [ - "controller_server", - "smoother_server", - "planner_server", - "behavior_server", - "bt_navigator", - "waypoint_follower", - "velocity_smoother", - ] - }, - {"use_sim_time": use_sim}, - ], - ), - Node( - package="nav2_controller", - executable="controller_server", - output="screen", - parameters=standard_params, - remappings=[("/cmd_vel", "/cmd_vel")], - ), - Node( - package="nav2_smoother", - executable="smoother_server", - name="smoother_server", - output="screen", - parameters=standard_params, - ), - Node( - package="nav2_planner", - executable="planner_server", - name="planner_server", - output="screen", - parameters=standard_params, - ), - Node( - package="nav2_behaviors", - executable="behavior_server", - name="behavior_server", - output="screen", - parameters=standard_params, - ), - Node( - package="nav2_bt_navigator", - executable="bt_navigator", - name="bt_navigator", - output="screen", - parameters=standard_params, - ), - Node( - package="nav2_waypoint_follower", - executable="waypoint_follower", - name="waypoint_follower", - output="screen", - parameters=standard_params, - ), - Node( - package="nav2_velocity_smoother", - executable="velocity_smoother", - name="velocity_smoother", - output="screen", - parameters=standard_params, - remappings=[ - ("/cmd_vel", "/cmd_vel_nav"), - ("/cmd_vel_smoothed", "/cmd_vel"), - ], - ), - Node( - package="nav2_map_server", - executable="map_server", - name="map_server", - output="screen", - parameters=[{"use_sim_time": use_sim, "yaml_filename": map_yaml_file}], - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager_localization", - output="screen", - parameters=[ - {"use_sim_time": use_sim}, - {"autostart": True}, - {"node_names": ["map_server"]}, - ], - ), - Node( - package="go2_sdk", - executable="go2_lidar_localization", - name="go2_lidar_localization", - output="screen", - parameters=[ - { - "base_frame": "base_link", - "odom_frame": "odom", - "laser_frame": "laser", - "laser_topic": "scan", - "global_localization_particles": global_localization_particles, - "use_sim_time": use_sim, - } - ], - ), - ] - - # Add the sim param timer if we're in simulation mode - if sim_param_timer: - nodes.append(sim_param_timer) - - return nodes - - -def generate_launch_description(): channel_type = LaunchConfiguration( "channel_type", default=EnvironmentVariable("LIDAR_CHANNEL_TYPE", default_value="serial"), @@ -332,6 +85,10 @@ def generate_launch_description(): "scan_mode", default=EnvironmentVariable("LIDAR_SCAN_MODE", default_value="Sensitivity"), ) + map_yaml_file = LaunchConfiguration( + "map_yaml_file", + default=EnvironmentVariable("MAP_YAML_FILE", default_value=""), + ) global_localization_particles = LaunchConfiguration( "global_localization_particles", default=EnvironmentVariable( @@ -391,6 +148,240 @@ def generate_launch_description(): default_value=use_sim, description="Whether to use simulation", ), - OpaqueFunction(function=configure_nav2_nodes), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_desc}], + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + condition=UnlessCondition(use_sim), + ), + # Node( + # package='rplidar_ros', + # executable='rplidar_node', + # name='rplidar_node', + # parameters=[{ + # 'channel_type': channel_type, + # 'serial_port': serial_port, + # 'serial_baudrate': serial_baudrate, + # 'frame_id': frame_id, + # 'inverted': inverted, + # 'angle_compensate': angle_compensate + # }], + # output='screen'), + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher_laser", + arguments=[ + "--x", + "0.2", + "--y", + "0", + "--z", + "0.05", + "--roll", + "0", + "--pitch", + "0", + "--yaw", + "3.14159", + "--frame-id", + "base_link", + "--child-frame-id", + "laser", + ], + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="pose_to_tf", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="cmd_vel_to_go2", + name="cmd_vel_to_go2", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="go2_sport_action", + name="go2_sport_action", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="waypoint_manager", + name="waypoint_manager", + output="screen", + ), + Node( + package="go2_sdk", + executable="go2_nav2_api", + name="go2_nav2_api_node", + output="screen", + ), + Node( + package="joy", + executable="joy_node", + name="joy_node", + output="screen", + condition=UnlessCondition(use_sim), + ), + # RB is the enable button + # The left joystick controls linear movement + # The right joystick controls angular movement + Node( + package="teleop_twist_joy", + executable="teleop_node", + name="teleop_twist_joy_node", + output="screen", + parameters=[ + { + "axis_linear.x": 1, + "axis_linear.y": 0, + "axis_angular.z": 3, + "enable_button": 10, + "scale_linear.x": 0.5, + "scale_angular.z": 0.5, + "enable_turbo_button": 9, + "scale_turbo_linear.x": 1.5, + "scale_turbo_angular.z": 2.0, + } + ], + condition=UnlessCondition(use_sim), + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + parameters=[ + {"use_sim_time": False}, + {"autostart": True}, + { + "node_names": [ + "controller_server", + "smoother_server", + "planner_server", + "behavior_server", + "bt_navigator", + "waypoint_follower", + "velocity_smoother", + ] + }, + {"use_sim_time": use_sim}, + ], + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + parameters=get_node_params( + "controller_server", nav2_config_file, use_sim + ), + remappings=[("/cmd_vel", "/cmd_vel")], + ), + Node( + package="nav2_smoother", + executable="smoother_server", + name="smoother_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + parameters=get_node_params("planner_server", nav2_config_file, use_sim), + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_velocity_smoother", + executable="velocity_smoother", + name="velocity_smoother", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + remappings=[ + ("/cmd_vel", "/cmd_vel_nav"), + ("/cmd_vel_smoothed", "/cmd_vel"), + ], + ), + Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + output="screen", + parameters=[{"use_sim_time": use_sim, "yaml_filename": map_yaml_file}], + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_localization", + output="screen", + parameters=[ + {"use_sim_time": use_sim}, + {"autostart": True}, + # {'node_names': ['map_server', 'amcl']} + {"node_names": ["map_server"]}, + ], + ), + # Node( + # package='nav2_amcl', + # executable='amcl', + # name='amcl', + # output='screen', + # parameters=[nav2_config_file] + # ) + # Custom Lidar Localization Node + Node( + package="go2_sdk", + executable="go2_lidar_localization", + name="go2_lidar_localization", + output="screen", + parameters=[ + { + "base_frame": "base_link", + "odom_frame": "odom", + "laser_frame": "laser", + "laser_topic": "scan", + "global_localization_particles": global_localization_particles, + "use_sim_time": use_sim, + } + ], + ), ] ) diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index 6d331a1..c9d9f5c 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -2,50 +2,53 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - ExecuteProcess, - OpaqueFunction, - TimerAction, -) +from launch.actions import DeclareLaunchArgument from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -# Simulation-specific parameter overrides as (node_name, param_path, value) tuples -# These are applied via ros2 param set when use_sim:=true -SIM_PARAM_OVERRIDES = [ - # Controller server - MPPI controller params - ("/controller_server", "FollowPath.vx_std", "0.2"), - ("/controller_server", "FollowPath.wz_std", "0.4"), - ("/controller_server", "FollowPath.vx_max", "0.5"), - ("/controller_server", "FollowPath.vx_min", "-0.35"), - ("/controller_server", "FollowPath.wz_max", "1.2"), - ("/controller_server", "FollowPath.ObstaclesCritic.cost_weight", "10.0"), - ( - "/controller_server", - "FollowPath.ObstaclesCritic.collision_margin_distance", - "0.05", - ), - ("/controller_server", "FollowPath.ObstaclesCritic.near_goal_distance", "0.4"), - ("/controller_server", "FollowPath.GoalAngleCritic.cost_power", "4"), - ("/controller_server", "FollowPath.PreferForwardCritic.cost_weight", "16.0"), - # Local costmap inflation layer - ("/local_costmap/local_costmap", "inflation_layer.cost_scaling_factor", "5.0"), - ("/local_costmap/local_costmap", "inflation_layer.inflation_radius", "0.30"), - # Global costmap inflation layer - ("/global_costmap/global_costmap", "inflation_layer.cost_scaling_factor", "6.0"), - ("/global_costmap/global_costmap", "inflation_layer.inflation_radius", "0.32"), - # Velocity smoother - ("/velocity_smoother", "max_velocity", "[0.5, 0.0, 1.2]"), - ("/velocity_smoother", "min_velocity", "[-0.35, 0.0, -1.2]"), - ("/velocity_smoother", "max_accel", "[2.0, 0.0, 2.5]"), - ("/velocity_smoother", "max_decel", "[-2.0, 0.0, -2.5]"), -] +SIM_PARAM_OVERRIDES = { + "controller_server": { + "FollowPath.vx_std": 0.2, + "FollowPath.wz_std": 0.4, + "FollowPath.vx_max": 0.5, + "FollowPath.vx_min": -0.35, + "FollowPath.wz_max": 1.2, + "FollowPath.ObstaclesCritic.cost_weight": 10.0, + "FollowPath.ObstaclesCritic.collision_margin_distance": 0.05, + "FollowPath.ObstaclesCritic.near_goal_distance": 0.4, + "FollowPath.GoalAngleCritic.cost_power": 4, + "FollowPath.PreferForwardCritic.cost_weight": 16.0, + # local costmap params + "inflation_layer.cost_scaling_factor": 5.0, + "inflation_layer.inflation_radius": 0.30, + }, + "planner_server": { + # globl costmap params + "inflation_layer.cost_scaling_factor": 6.0, + "inflation_layer.inflation_radius": 0.32, + }, + "velocity_smoother": { + "max_velocity": [0.5, 0.0, 1.2], + "min_velocity": [-0.35, 0.0, -1.2], + "max_accel": [2.0, 0.0, 2.5], + "max_decel": [-2.0, 0.0, -2.5], + }, +} +def get_node_params(node_name: str, base_config: str, use_sim: bool): + """ + Merge base config with simulation overrides if needed. + """ + params = [base_config, {"use_sim_time": use_sim}] -def configure_slam_nodes(context, *args, **kwargs): - """Configure SLAM and navigation nodes with optional simulation overrides.""" + if use_sim and node_name in SIM_PARAM_OVERRIDES: + params.append(SIM_PARAM_OVERRIDES[node_name]) + + return params + + +def generate_launch_description(): pkg_dir = get_package_share_directory("go2_sdk") urdf_file = os.path.join(pkg_dir, "urdf", "go2.urdf") @@ -58,224 +61,6 @@ def configure_slam_nodes(context, *args, **kwargs): pkg_dir, "config", "m_explorer_ros2_params.yaml" ) - # Resolve use_sim at launch time - use_sim_str = LaunchConfiguration("use_sim").perform(context) - is_sim = use_sim_str.lower() == "true" - use_sim = LaunchConfiguration("use_sim") - - # Build ros2 param set commands for simulation overrides - sim_param_commands = [] - if is_sim: - for node_name, param_path, value in SIM_PARAM_OVERRIDES: - sim_param_commands.append( - ExecuteProcess( - cmd=["ros2", "param", "set", node_name, param_path, value], - output="screen", - ) - ) - - # Wrap sim param commands in a TimerAction to wait for nodes to be ready - sim_param_timer = ( - TimerAction( - period=10.0, # Wait for Nav2 nodes to be fully active - actions=sim_param_commands, - ) - if sim_param_commands - else None - ) - - nodes = [ - Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="screen", - parameters=[{"robot_description": robot_desc}], - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="joint_state_publisher", - name="joint_state_publisher", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher_laser", - arguments=[ - "--x", - "0.2", - "--y", - "0", - "--z", - "0.05", - "--roll", - "0", - "--pitch", - "0", - "--yaw", - "3.14159", - "--frame-id", - "base_link", - "--child-frame-id", - "laser", - ], - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="pose_to_tf", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="cmd_vel_to_go2", - name="cmd_vel_to_go2", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="go2_sport_action", - name="go2_sport_action", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="go2_sdk", - executable="waypoint_manager", - name="waypoint_manager", - output="screen", - ), - Node( - package="joy", - executable="joy_node", - name="joy_node", - output="screen", - condition=UnlessCondition(use_sim), - ), - Node( - package="teleop_twist_joy", - executable="teleop_node", - name="teleop_twist_joy_node", - output="screen", - parameters=[ - { - "axis_linear.x": 1, - "axis_linear.y": 0, - "axis_angular.z": 3, - "enable_button": 10, - "scale_linear.x": 0.5, - "scale_angular.z": 1.0, - "enable_turbo_button": 9, - "scale_turbo_linear.x": 1.5, - "scale_turbo_angular.z": 2.0, - } - ], - condition=UnlessCondition(use_sim), - ), - Node( - package="slam_toolbox", - executable="sync_slam_toolbox_node", - name="slam_toolbox", - parameters=[slam_config_file, {"use_sim_time": use_sim}], - output="screen", - ), - Node( - package="nav2_lifecycle_manager", - executable="lifecycle_manager", - name="lifecycle_manager_navigation", - output="screen", - parameters=[ - {"use_sim_time": use_sim}, - {"autostart": True}, - { - "node_names": [ - "controller_server", - "smoother_server", - "planner_server", - "behavior_server", - "bt_navigator", - "waypoint_follower", - "velocity_smoother", - ] - }, - ], - ), - Node( - package="nav2_controller", - executable="controller_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - remappings=[("/cmd_vel", "/cmd_vel")], - ), - Node( - package="nav2_smoother", - executable="smoother_server", - name="smoother_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_planner", - executable="planner_server", - name="planner_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_behaviors", - executable="behavior_server", - name="behavior_server", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_bt_navigator", - executable="bt_navigator", - name="bt_navigator", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_waypoint_follower", - executable="waypoint_follower", - name="waypoint_follower", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - ), - Node( - package="nav2_velocity_smoother", - executable="velocity_smoother", - name="velocity_smoother", - output="screen", - parameters=[nav2_config_file, {"use_sim_time": use_sim}], - remappings=[ - ("/cmd_vel", "/cmd_vel_nav"), - ("/cmd_vel_smoothed", "/cmd_vel"), - ], - ), - Node( - package="frontier_explorer", - executable="explore", - name="frontier_explorer", - output="screen", - parameters=[m_explorer_config_file, {"use_sim_time": use_sim}], - ), - ] - - # Add the sim param timer if we're in simulation mode - if sim_param_timer: - nodes.append(sim_param_timer) - - return nodes - - -def generate_launch_description(): channel_type = LaunchConfiguration("channel_type", default="serial") serial_port = LaunchConfiguration("serial_port", default="/dev/ttyUSB0") serial_baudrate = LaunchConfiguration("serial_baudrate", default="115200") @@ -333,6 +118,225 @@ def generate_launch_description(): default_value=use_sim, description="Whether to use simulation", ), - OpaqueFunction(function=configure_slam_nodes), + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": robot_desc}], + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="joint_state_publisher", + name="joint_state_publisher", + output="screen", + condition=UnlessCondition(use_sim), + ), + # Node( + # package='rplidar_ros', + # executable='rplidar_node', + # name='rplidar_node', + # parameters=[{ + # 'channel_type': channel_type, + # 'serial_port': serial_port, + # 'serial_baudrate': serial_baudrate, + # 'frame_id': frame_id, + # 'inverted': inverted, + # 'angle_compensate': angle_compensate + # }], + # output='screen'), + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher_laser", + arguments=[ + "--x", + "0.2", + "--y", + "0", + "--z", + "0.05", + "--roll", + "0", + "--pitch", + "0", + "--yaw", + "3.14159", + "--frame-id", + "base_link", + "--child-frame-id", + "laser", + ], + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="pose_to_tf", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="cmd_vel_to_go2", + name="cmd_vel_to_go2", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="go2_sport_action", + name="go2_sport_action", + output="screen", + condition=UnlessCondition(use_sim), + ), + Node( + package="go2_sdk", + executable="waypoint_manager", + name="waypoint_manager", + output="screen", + ), + Node( + package="joy", + executable="joy_node", + name="joy_node", + output="screen", + condition=UnlessCondition(use_sim), + ), + # RB is the enable button + # The left joystick controls linear movement + # The right joystick controls angular movement + Node( + package="teleop_twist_joy", + executable="teleop_node", + name="teleop_twist_joy_node", + output="screen", + parameters=[ + { + "axis_linear.x": 1, + "axis_linear.y": 0, + "axis_angular.z": 3, + "enable_button": 10, + "scale_linear.x": 0.5, + "scale_angular.z": 1.0, + "enable_turbo_button": 9, + "scale_turbo_linear.x": 1.5, + "scale_turbo_angular.z": 2.0, + } + ], + condition=UnlessCondition(use_sim), + ), + Node( + package="slam_toolbox", + executable="sync_slam_toolbox_node", + name="slam_toolbox", + parameters=[slam_config_file, {"use_sim_time": use_sim}], + output="screen", + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + parameters=[ + {"use_sim_time": use_sim}, + {"autostart": True}, + { + "node_names": [ + "controller_server", + "smoother_server", + "planner_server", + "behavior_server", + "bt_navigator", + "waypoint_follower", + "velocity_smoother", + ] + }, + ], + ), + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + parameters=get_node_params( + "controller_server", nav2_config_file, use_sim + ), + remappings=[ + ("/cmd_vel", "/cmd_vel"), + ], + ), + Node( + package="nav2_smoother", + executable="smoother_server", + name="smoother_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + parameters=get_node_params("planner_server", nav2_config_file, use_sim), + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + parameters=[nav2_config_file, {"use_sim_time": use_sim}], + ), + Node( + package="nav2_velocity_smoother", + executable="velocity_smoother", + name="velocity_smoother", + output="screen", + parameters=get_node_params( + "velocity_smoother", nav2_config_file, use_sim + ), + remappings=[ + ("/cmd_vel", "/cmd_vel_nav"), + ("/cmd_vel_smoothed", "/cmd_vel"), + ], + ), + # Node( + # package='nav2_lifecycle_manager', + # executable='lifecycle_manager', + # name='lifecycle_manager_localization', + # output='screen', + # parameters=[ + # {'use_sim_time': False}, + # {'autostart': True}, + # {'node_names': ['amcl']}] + # ), + # Node( + # package='nav2_amcl', + # executable='amcl', + # name='amcl', + # output='screen', + # parameters=[nav2_config_file] + # ) + Node( + package="frontier_explorer", + executable="explore", + name="frontier_explorer", + output="screen", + parameters=[m_explorer_config_file, {"use_sim_time": use_sim}], + ), ] ) From b2f6e7008689ac46bcd449c8b9cd8b6aac8ba61d Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 14:08:29 -0800 Subject: [PATCH 09/13] Fix navigation launch typos, and remove unused nav2 sim parameters. --- go2_gazebo_sim/go2_description/CMakeLists.txt | 5 - .../go2_gazebo_sim/launch/go2_launch.py | 4 +- go2_sdk/config/nav2_params.yaml | 4 +- go2_sdk/config/nav2_params_sim.yaml | 453 ------------------ go2_sdk/launch/nav2_launch.py | 2 +- go2_sdk/launch/slam_launch.py | 2 +- 6 files changed, 4 insertions(+), 466 deletions(-) delete mode 100644 go2_sdk/config/nav2_params_sim.yaml diff --git a/go2_gazebo_sim/go2_description/CMakeLists.txt b/go2_gazebo_sim/go2_description/CMakeLists.txt index 93316cf..207f272 100644 --- a/go2_gazebo_sim/go2_description/CMakeLists.txt +++ b/go2_gazebo_sim/go2_description/CMakeLists.txt @@ -31,11 +31,6 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY - models - DESTINATION - share/${PROJECT_NAME}) - install(DIRECTORY tags DESTINATION diff --git a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py index ee931c1..bc69612 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py +++ b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py @@ -34,6 +34,7 @@ def generate_launch_description(): links_config = os.path.join(go2_gazebo_sim, "config/links/links.yaml") default_model_path = os.path.join(go2_description, "urdf/unitree_go2_robot.xacro") default_world_path = os.path.join(go2_description, "worlds/home_world.sdf") + aruco_model_path = os.path.join(go2_description, "models/aruco_marker/model.sdf") # Add go2_description/models to GZ_SIM_RESOURCE_PATH go2_description_models = os.path.join(go2_description, "models") @@ -267,9 +268,6 @@ def generate_launch_description(): }.items(), ) - # Spawn ArUco Marker - # We use the absolute path to the model.sdf to avoid model:// URI resolution issues - aruco_model_path = os.path.join(go2_description, "models/aruco_marker/model.sdf") gazebo_spawn_aruco = Node( package="ros_gz_sim", diff --git a/go2_sdk/config/nav2_params.yaml b/go2_sdk/config/nav2_params.yaml index 936673c..75eb5c8 100644 --- a/go2_sdk/config/nav2_params.yaml +++ b/go2_sdk/config/nav2_params.yaml @@ -240,8 +240,7 @@ local_costmap: height: 4 resolution: 0.05 transform_tolerance: 1.0 - # Slightly smaller footprint to prevent self-collision in costmap - footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" # Robot shape [x,y] for collision checking + footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] static_layer: @@ -318,7 +317,6 @@ global_costmap: robot_base_frame: base_link use_sim_time: False transform_tolerance: 1.0 - # Slightly smaller footprint footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" resolution: 0.1 track_unknown_space: False diff --git a/go2_sdk/config/nav2_params_sim.yaml b/go2_sdk/config/nav2_params_sim.yaml deleted file mode 100644 index 6e2294d..0000000 --- a/go2_sdk/config/nav2_params_sim.yaml +++ /dev/null @@ -1,453 +0,0 @@ -amcl: - ros__parameters: - use_sim_time: False - alpha1: 0.05 - alpha2: 0.1 - alpha3: 0.05 - alpha4: 0.05 - alpha5: 0.05 - base_frame_id: "base_link" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: False - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 12.0 - laser_min_range: 0.1 - laser_model_type: "likelihood_field" - max_beams: 180 - max_particles: 8000 - min_particles: 2000 - odom_frame_id: "odom" - pf_err: 0.01 - pf_z: 0.95 - recovery_alpha_fast: 0.3 - recovery_alpha_slow: 0.01 - resample_interval: 1 - robot_model_type: "nav2_amcl::DifferentialMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: True - transform_tolerance: 1.0 - update_min_a: 0.005 - update_min_d: 0.02 - z_hit: 0.85 - z_max: 0.05 - z_rand: 0.05 - z_short: 0.15 - scan_topic: scan - set_initial_pose: True - initial_pose_x: 0.0 - initial_pose_y: 0.0 - initial_pose_z: 0.0 - initial_pose_a: 0.0 - -bt_navigator: - ros__parameters: - use_sim_time: False - global_frame: map - transform_tolerance: 1.0 - robot_base_frame: base_link - odom_topic: /odom - bt_loop_duration: 10 - default_server_timeout: 30 - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node - -controller_server: - ros__parameters: - use_sim_time: False - controller_frequency: 30.0 - min_x_velocity_threshold: 0.05 - min_y_velocity_threshold: 0.001 - min_theta_velocity_threshold: 0.1 - failure_tolerance: 0.8 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] - controller_plugins: ["FollowPath"] - - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.1 - movement_time_allowance: 45.0 - - general_goal_checker: - stateful: True - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.35 - yaw_goal_tolerance: 0.35 - - FollowPath: - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 48 - model_dt: 0.07 - batch_size: 1500 - vx_std: 0.2 # Velocity variance (exploration noise) - vy_std: 0.0 - wz_std: 0.4 # Angular velocity variance - vx_max: 0.5 # Absolute max forward speed (m/s) - vx_min: -0.35 # Max reverse speed (m/s) - vy_max: 0.0 - wz_max: 1.2 # Max angular velocity (rad/s) - iteration_count: 1 # Optimization iterations (1 is efficient) - prune_distance: 1.5 # Remove path points closer than this - transform_tolerance: 0.5 - temperature: 0.4 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: False - regenerate_noises: True - - critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic", "ObstaclesCritic"] - - ConstraintCritic: # Penalizes violating velocity/accel limits - enabled: True - cost_power: 1 - cost_weight: 4.0 - - GoalCritic: # Penalizes distance to goal - enabled: True - cost_power: 1 - cost_weight: 10.0 - threshold_to_consider: 1.2 - - ObstaclesCritic: # Penalizes collisions (CRITICAL) - enabled: True - cost_power: 1 - cost_weight: 10.0 # Reduced from 18.0 to allow closer approach - consider_footprint: False - collision_cost: 10000.0 - collision_margin_distance: 0.05 # Reduced from 0.12 for tighter navigation - near_goal_distance: 0.4 - - GoalAngleCritic: - enabled: True - cost_power: 4 - cost_weight: 10.0 - threshold_to_consider: 0.6 - - PreferForwardCritic: - enabled: True - cost_power: 1 - cost_weight: 16.0 - threshold_to_consider: 0.5 - - CostCritic: # Penalizes high-cost areas - enabled: True - cost_power: 1 - cost_weight: 3.5 - critical_cost: 300.0 - consider_footprint: True - collision_cost: 1000000.0 - near_goal_distance: 0.5 - trajectory_point_step: 2 - - PathAlignCritic: - enabled: True - cost_power: 1 - cost_weight: 32.0 - max_path_occupancy_ratio: 0.07 - trajectory_point_step: 3 - threshold_to_consider: 0.6 - offset_from_furthest: 15 - use_path_orientations: False - - PathFollowCritic: - enabled: True - cost_power: 1 - cost_weight: 20.0 - offset_from_furthest: 6 - threshold_to_consider: 1.2 - - PathAngleCritic: - enabled: True - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.6 - max_angle_to_furthest: 1.2 - mode: 0 - - noise_generator_config: - default: - time_steps: 48 - batch_size: 1500 - vx_std: 0.15 - wz_std: 0.35 - vx_mu: 0.0 - wz_mu: 0.0 - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 8.0 - publish_frequency: 4.0 - global_frame: odom - robot_base_frame: base_link - use_sim_time: False - rolling_window: True - width: 4 - height: 4 - resolution: 0.05 - transform_tolerance: 1.0 - # Slightly smaller footprint to prevent self-collision in costmap - footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" # Robot shape [x,y] for collision checking - plugins: ["static_layer", "obstacle_layer", "stvl_layer", "inflation_layer"] - - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - footprint_clearing_enabled: True # Clear footprint area - max_obstacle_height: 2.0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - min_obstacle_height: 0.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 3.5 - raytrace_min_range: 0.0 - obstacle_max_range: 3.0 - obstacle_min_range: 0.15 - - stvl_layer: - plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" - enabled: True - voxel_decay: 1.5 # Faster decay - decay_model: 0 - voxel_size: 0.05 - track_unknown_space: False - observation_persistence: 0.0 - max_obstacle_height: 2.5 - min_obstacle_height: 0.18 # Slightly higher to avoid ground noise - obstacle_max_range: 2.5 - obstacle_min_range: 0.25 - origin_z: 0.0 - publish_voxel_map: True - transform_tolerance: 1.0 - mapping_mode: False - map_save_duration: 60.0 - combination_method: 1 - update_footprint_enabled: True - observation_sources: lidar_4d - lidar_4d: - data_type: PointCloud2 - topic: /utlidar/cloud_deskewed - marking: True - clearing: True - min_obstacle_height: 0.18 - max_obstacle_height: 2.5 - expected_update_rate: 15.0 - observation_persistence: 0.0 - inf_is_valid: False - filter: "voxel" - voxel_min_points: 3 - clear_after_reading: True - enabled: True - model_type: 1 - - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 5.0 # Faster decay (less repulsion) - inflation_radius: 0.30 # Original value - - always_send_full_costmap: True - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 2.0 # Reduced - publish_frequency: 1.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: False - transform_tolerance: 1.0 - # Slightly smaller footprint - footprint: "[[0.35, 0.20], [0.35, -0.20], [-0.35, -0.20], [-0.35, 0.20]]" - resolution: 0.1 - track_unknown_space: False - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - - obstacle_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - footprint_clearing_enabled: True - max_obstacle_height: 2.0 - observation_sources: scan - scan: - topic: /scan - max_obstacle_height: 2.0 - min_obstacle_height: 0.0 - clearing: True - marking: True - data_type: "LaserScan" - raytrace_max_range: 4.0 - raytrace_min_range: 0.0 - obstacle_max_range: 3.5 - obstacle_min_range: 0.15 - - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 6.0 # Faster decay (less repulsion) - inflation_radius: 0.32 # Original value - - always_send_full_costmap: True - -map_server: - ros__parameters: - use_sim_time: False - yaml_filename: "" - -map_saver: - ros__parameters: - use_sim_time: False - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 5.0 - transform_tolerance: 1.0 - use_sim_time: False - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_smac_planner/SmacPlanner2D" - tolerance: 0.5 - downsample_costmap: False - downsampling_factor: 1 - allow_unknown: True # Allow planning through unknown space - max_iterations: 1000000 - max_on_approach_iterations: 1000 - smooth_path: True - #Add cost_travel_multiplier to prefer free space - cost_travel_multiplier: 2.0 # Higher = prefer low cost areas vs short path - -smoother_server: - ros__parameters: - use_sim_time: False - smoother_plugins: ["simple_smoother"] - simple_smoother: - plugin: "nav2_smoother::SimpleSmoother" - tolerance: 1.0e-10 - max_its: 1000 - do_refinement: True - -behavior_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] - spin: - plugin: "nav2_behaviors/Spin" - backup: - plugin: "nav2_behaviors/BackUp" - backup_dist: 0.4 - backup_speed: 0.12 - time_allowance: 20.0 - drive_on_heading: - plugin: "nav2_behaviors/DriveOnHeading" - wait: - plugin: "nav2_behaviors/Wait" - assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" - global_frame: odom - robot_base_frame: base_link - transform_tolerance: 1.0 # Increased - use_sim_time: False - simulate_ahead_time: 3.0 - max_rotational_vel: 0.6 - min_rotational_vel: 0.15 - rotational_acc_lim: 1.8 - -robot_state_publisher: - ros__parameters: - use_sim_time: False - -waypoint_follower: - ros__parameters: - use_sim_time: False - loop_rate: 20 - stop_on_failure: False - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - use_sim_time: False - smoothing_frequency: 20.0 - scale_velocities: False - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 1.2] # Kinematic hard limits [vx, vy, wz] - min_velocity: [-0.35, 0.0, -1.2] - max_accel: [2.0, 0.0, 2.5] # Max acceleration [ax, ay, az] - max_decel: [-2.0, 0.0, -2.5] - odom_topic: "odom" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index 064a376..8ee654c 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -25,7 +25,7 @@ "inflation_layer.inflation_radius": 0.30, }, "planner_server": { - # globl costmap params + # global costmap params "inflation_layer.cost_scaling_factor": 6.0, "inflation_layer.inflation_radius": 0.32, }, diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index c9d9f5c..f2d26fc 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -24,7 +24,7 @@ "inflation_layer.inflation_radius": 0.30, }, "planner_server": { - # globl costmap params + # global costmap params "inflation_layer.cost_scaling_factor": 6.0, "inflation_layer.inflation_radius": 0.32, }, From b7b144eaa4c532f615910f6d94f3f01e952cbfcd Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 14:10:00 -0800 Subject: [PATCH 10/13] Fix lint --- go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py | 1 - go2_sdk/launch/nav2_launch.py | 3 ++- go2_sdk/launch/slam_launch.py | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py index bc69612..c809566 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py +++ b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py @@ -268,7 +268,6 @@ def generate_launch_description(): }.items(), ) - gazebo_spawn_aruco = Node( package="ros_gz_sim", executable="create", diff --git a/go2_sdk/launch/nav2_launch.py b/go2_sdk/launch/nav2_launch.py index 8ee654c..1f97c61 100644 --- a/go2_sdk/launch/nav2_launch.py +++ b/go2_sdk/launch/nav2_launch.py @@ -7,7 +7,6 @@ from launch.substitutions import EnvironmentVariable, LaunchConfiguration from launch_ros.actions import Node - SIM_PARAM_OVERRIDES = { "controller_server": { "FollowPath.vx_std": 0.2, @@ -37,6 +36,7 @@ }, } + def get_node_params(node_name: str, base_config: str, use_sim: bool): """ Merge base config with simulation overrides if needed. @@ -48,6 +48,7 @@ def get_node_params(node_name: str, base_config: str, use_sim: bool): return params + def generate_launch_description(): pkg_dir = get_package_share_directory("go2_sdk") diff --git a/go2_sdk/launch/slam_launch.py b/go2_sdk/launch/slam_launch.py index f2d26fc..510bca4 100644 --- a/go2_sdk/launch/slam_launch.py +++ b/go2_sdk/launch/slam_launch.py @@ -36,6 +36,7 @@ }, } + def get_node_params(node_name: str, base_config: str, use_sim: bool): """ Merge base config with simulation overrides if needed. From 04a87b1e51b267cb5bac3b672611ee7cf2b2fcc6 Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 15:39:18 -0800 Subject: [PATCH 11/13] Refactor battery monitor, and enhance low state simulation with dynamic battery drain and `/lf/lowstate` publishing. --- go2_auto_dock/go2_auto_dock/go2_dock.py | 2 +- .../go2_auto_dock/go2_nav_to_charger_final.py | 4 +- go2_auto_dock/setup.py | 3 +- .../go2_gazebo_sim/go2_lowstate.py | 261 ++++++++++++------ go2_sdk/go2_sdk/go2_dock.py | 106 ------- go2_sdk/launch/go2_charge.launch.py | 10 +- 6 files changed, 192 insertions(+), 194 deletions(-) delete mode 100644 go2_sdk/go2_sdk/go2_dock.py diff --git a/go2_auto_dock/go2_auto_dock/go2_dock.py b/go2_auto_dock/go2_auto_dock/go2_dock.py index a1386d7..ba885e2 100644 --- a/go2_auto_dock/go2_auto_dock/go2_dock.py +++ b/go2_auto_dock/go2_auto_dock/go2_dock.py @@ -78,7 +78,7 @@ def trigger_charging_navigation(self): try: # Launch the navigation script - subprocess.Popen(["ros2", "run", "go2_auto_dock", "go2_dock"]) + subprocess.Popen(["ros2", "run", "go2_auto_dock", "go2_nav_to_charger"]) self.charging_navigation_triggered = True self.get_logger().info( "Navigation to charger script launched successfully." diff --git a/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py b/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py index 06f57a8..36b8f69 100644 --- a/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py +++ b/go2_auto_dock/go2_auto_dock/go2_nav_to_charger_final.py @@ -35,8 +35,8 @@ def __init__(self): self.nav_client = ActionClient(self, NavigateToPose, "navigate_to_pose") # Define goal position and orientation (bearing angle) - self.goal_position = {"x": 0.01612, "y": 0.1392, "z": 0.0} - self.goal_orientation = {"x": 0.0, "y": 0.0, "z": 0.9512, "w": 0.3085} + self.goal_position = {"x": -0.9578, "y": -1.952, "z": 0.0} + self.goal_orientation = {"x": 0.0, "y": 0.0, "z": 0.99, "w": -0.067} # Precompute goal yaw (deg) self.goal_yaw_deg = yaw_from_quaternion( diff --git a/go2_auto_dock/setup.py b/go2_auto_dock/setup.py index 01a50a2..227a048 100644 --- a/go2_auto_dock/setup.py +++ b/go2_auto_dock/setup.py @@ -32,7 +32,8 @@ "go2_apriltag_detector = go2_auto_dock.go2_apriltag_detector:main", "go2_tag_follower = go2_auto_dock.go2_tag_follower:main", "go2_tag_charger = go2_auto_dock.go2_tag_charger:main", - "go2_dock = go2_auto_dock.go2_nav_to_charger_final:main", + "go2_battery_monitor = go2_auto_dock.go2_dock:main", + "go2_nav_to_charger = go2_auto_dock.go2_nav_to_charger_final:main", "go2_camera_publisher = go2_auto_dock.go2_camera_with_adjustable_publisher:main", ], }, diff --git a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py index 021317c..c805e18 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py +++ b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py @@ -1,7 +1,15 @@ #!/usr/bin/env python3 +""" +A ROS2 node that publishes mock LowState messages for Unitree Go2 simulation. +This provides simulated robot state data including IMU, motor, and battery information. +Battery simulation auto-detects movement from cmd_vel for realistic drain behavior. +""" import rclpy +from geometry_msgs.msg import Twist +from rcl_interfaces.msg import SetParametersResult from rclpy.node import Node +from rclpy.parameter import Parameter from sensor_msgs.msg import JointState from unitree_go.msg import BmsState, IMUState, LowState, MotorState @@ -11,26 +19,96 @@ class Go2LowStateNode(Node): """ A ROS2 node that publishes mock LowState messages for Unitree Go2 simulation. This provides simulated robot state data including IMU, motor, and battery information. + Battery drain is automatically calculated based on cmd_vel activity. """ + # Expected joint names from Gazebo + EXPECTED_JOINT_NAMES = [ + "rf_hip_joint", + "lf_lower_leg_joint", + "rf_lower_leg_joint", + "lf_upper_leg_joint", + "rh_hip_joint", + "rf_upper_leg_joint", + "rh_upper_leg_joint", + "rh_lower_leg_joint", + "lh_hip_joint", + "lf_hip_joint", + "lh_upper_leg_joint", + "lh_lower_leg_joint", + ] + + # Mapping from Gazebo joint order to Unitree convention: + # [rf_hip, rf_upper, rf_lower, lf_hip, lf_upper, lf_lower, + # rh_hip, rh_upper, rh_lower, lh_hip, lh_upper, lh_lower] + UNITREE_JOINT_INDICES = [0, 5, 2, 9, 3, 1, 4, 6, 7, 8, 10, 11] + def __init__(self): super().__init__("go2_lowstate_node") + # Declare battery simulation parameters + self.declare_parameter("soc", 80.0) + self.declare_parameter("drain_rate", 0.01) + self.declare_parameter("idle_drain_rate", 0.001) + self.declare_parameter("charge_rate", 0.05) + self.declare_parameter("is_charging", False) + self.declare_parameter("velocity_threshold", 0.01) + self.declare_parameter("cmd_vel_timeout", 0.5) + + # Publishers self.lowstate_publisher = self.create_publisher(LowState, "/lowstate", 10) + self.lf_lowstate_publisher = self.create_publisher(LowState, "/lf/lowstate", 10) + # Subscribers self.joint_state_subscriber = self.create_subscription( JointState, "/joint_states", self.joint_state_callback, 10 ) + self.cmd_vel_subscriber = self.create_subscription( + Twist, "/cmd_vel", self.cmd_vel_callback, 10 + ) + # Timer for publishing at 100Hz self.timer = self.create_timer(0.01, self.publish_lowstate) + # State variables self.tick_counter = 0 - self.latest_joint_state = None + self.cmd_vel_moving = False + self.last_cmd_vel_time = self.get_clock().now() + + # Parameter callback for runtime changes + self.add_on_set_parameters_callback(self.parameter_callback) self.get_logger().info("LowState Mock Node initialized") - self.get_logger().info("Publishing to: /lowstate at 100Hz") - self.get_logger().info("Subscribing to: /joint_states") + self.get_logger().info("Publishing to: /lowstate and /lf/lowstate at 100Hz") + self.get_logger().info("Subscribing to: /joint_states, /cmd_vel") + self.get_logger().info( + f"Battery: SoC={self.get_parameter('soc').value}%, " + f"drain_rate={self.get_parameter('drain_rate').value}%/tick" + ) + + def parameter_callback(self, params) -> SetParametersResult: + """ + Callback for handling parameter changes at runtime. + + Parameters: + ----------- + params : list + List of parameter changes + + Returns: + -------- + SetParametersResult + Result indicating success or failure + """ + for param in params: + if param.name == "is_charging": + status = "STARTED" if param.value else "STOPPED" + self.get_logger().info(f"Charging {status}") + elif param.name == "soc": + self.get_logger().info(f"SoC manually set to {param.value:.1f}%") + + return SetParametersResult(successful=True) def joint_state_callback(self, msg: JointState): """ @@ -43,6 +121,22 @@ def joint_state_callback(self, msg: JointState): """ self.latest_joint_state = msg + def cmd_vel_callback(self, msg: Twist): + """ + Callback function for velocity commands to detect robot movement. + + Parameters: + ----------- + msg : Twist + Velocity command message + """ + threshold = self.get_parameter("velocity_threshold").value + linear_speed = (msg.linear.x**2 + msg.linear.y**2) ** 0.5 + angular_speed = abs(msg.angular.z) + + self.cmd_vel_moving = linear_speed > threshold or angular_speed > threshold + self.last_cmd_vel_time = self.get_clock().now() + def get_unitree_joint_data(self): """ Convert Gazebo joint states to Unitree motor order. @@ -50,30 +144,16 @@ def get_unitree_joint_data(self): Returns: -------- tuple - (positions, velocities) in Unitree order, or None if no data available + (positions, velocities) in Unitree order, or (None, None) if no data available """ if self.latest_joint_state is None: return None, None - # Expected joint names from Gazebo in this order: - expected_names = [ - "rf_hip_joint", - "lf_lower_leg_joint", - "rf_lower_leg_joint", - "lf_upper_leg_joint", - "rh_hip_joint", - "rf_upper_leg_joint", - "rh_upper_leg_joint", - "rh_lower_leg_joint", - "lh_hip_joint", - "lf_hip_joint", - "lh_upper_leg_joint", - "lh_lower_leg_joint", - ] - name_to_index = {name: i for i, name in enumerate(self.latest_joint_state.name)} - missing_joints = [name for name in expected_names if name not in name_to_index] + missing_joints = [ + name for name in self.EXPECTED_JOINT_NAMES if name not in name_to_index + ] if missing_joints: self.get_logger().warn(f"Missing joints: {missing_joints}") return None, None @@ -81,7 +161,7 @@ def get_unitree_joint_data(self): gazebo_positions = [] gazebo_velocities = [] - for joint_name in expected_names: + for joint_name in self.EXPECTED_JOINT_NAMES: idx = name_to_index[joint_name] gazebo_positions.append(self.latest_joint_state.position[idx]) gazebo_velocities.append( @@ -90,24 +170,58 @@ def get_unitree_joint_data(self): else 0.0 ) - # Reorder to Unitree convention: [rf_hip, rf_upper, rf_lower, lf_hip, lf_upper, lf_lower, - # rh_hip, rh_upper, rh_lower, lh_hip, lh_upper, lh_lower] - # Mapping from gazebo order to unitree order: - unitree_indices = [0, 5, 2, 9, 3, 1, 4, 6, 7, 8, 10, 11] - - unitree_positions = [gazebo_positions[i] for i in unitree_indices] - unitree_velocities = [gazebo_velocities[i] for i in unitree_indices] + unitree_positions = [gazebo_positions[i] for i in self.UNITREE_JOINT_INDICES] + unitree_velocities = [gazebo_velocities[i] for i in self.UNITREE_JOINT_INDICES] return unitree_positions, unitree_velocities - def create_mock_lowstate(self) -> LowState: + def update_battery_state(self): + """ + Update battery state based on robot activity. + + Returns: + -------- + tuple + (soc, current_ma, is_moving) - current battery state + """ + soc = self.get_parameter("soc").value + is_charging = self.get_parameter("is_charging").value + timeout = self.get_parameter("cmd_vel_timeout").value + + # Check if robot is moving based on cmd_vel timeout + time_since_cmd = ( + self.get_clock().now() - self.last_cmd_vel_time + ).nanoseconds / 1e9 + is_moving = self.cmd_vel_moving and time_since_cmd < timeout + + # Don't drain while charging + if is_charging: + is_moving = False + + # Update SoC based on activity + if is_charging: + soc = min(100.0, soc + self.get_parameter("charge_rate").value) + current_ma = 3000 # Charging current + elif is_moving: + soc = max(0.0, soc - self.get_parameter("drain_rate").value) + current_ma = -5000 # High drain while moving + else: + soc = max(0.0, soc - self.get_parameter("idle_drain_rate").value) + current_ma = -500 # Low idle drain + + # Update the parameter with new SoC value + self.set_parameters([Parameter("soc", value=soc)]) + + return soc, current_ma, is_moving + + def create_lowstate_message(self) -> LowState: """ - Create a mock LowState message based on the provided sample data. + Create a LowState message with current robot state. Returns: -------- LowState - A populated LowState message with mock data + A populated LowState message with current data """ msg = LowState() @@ -128,7 +242,6 @@ def create_mock_lowstate(self) -> LowState: msg.imu_state.temperature = 0 # Motor States (20 motors total, 12 active + 8 inactive) - # Get current joint data from Gazebo unitree_positions, unitree_velocities = self.get_unitree_joint_data() # Use default values if no joint data is available yet @@ -136,42 +249,22 @@ def create_mock_lowstate(self) -> LowState: unitree_positions = [0.0] * 12 unitree_velocities = [0.0] * 12 - # Motor data with realistic temperatures and small torque estimates - motor_data = [] - for i in range(12): - motor_data.append( - { - "mode": 1, - "q": unitree_positions[i], - "dq": unitree_velocities[i], - "ddq": 0.0, - "tau_est": 0.025 + (i % 3) * 0.01, # Small varying torque estimates - "q_raw": 0.0, - "dq_raw": 0.0, - "ddq_raw": 0.0, - "temperature": 26 + (i % 6), # Varying temps 26-31°C - "lost": 0, - "reserve": [0, 588], - } - ) - # Create motor state array with exactly 20 elements motor_states = [] for i in range(20): motor = MotorState() if i < 12: - data = motor_data[i] - motor.mode = data["mode"] - motor.q = data["q"] - motor.dq = data["dq"] - motor.ddq = data["ddq"] - motor.tau_est = data["tau_est"] - motor.q_raw = data["q_raw"] - motor.dq_raw = data["dq_raw"] - motor.ddq_raw = data["ddq_raw"] - motor.temperature = data["temperature"] - motor.lost = data["lost"] - motor.reserve = data["reserve"] + motor.mode = 1 + motor.q = unitree_positions[i] + motor.dq = unitree_velocities[i] + motor.ddq = 0.0 + motor.tau_est = 0.025 + (i % 3) * 0.01 # Small varying torque estimates + motor.q_raw = 0.0 + motor.dq_raw = 0.0 + motor.ddq_raw = 0.0 + motor.temperature = 26 + (i % 6) # Varying temps 26-31°C + motor.lost = 0 + motor.reserve = [0, 588] else: motor.mode = 0 motor.q = 0.0 @@ -189,13 +282,15 @@ def create_mock_lowstate(self) -> LowState: msg.motor_state = motor_states - # BMS State + # Battery State (dynamic based on movement) + soc, current_ma, is_moving = self.update_battery_state() + msg.bms_state = BmsState() msg.bms_state.version_high = 1 msg.bms_state.version_low = 18 msg.bms_state.status = 8 - msg.bms_state.soc = 100 - msg.bms_state.current = -2752 + msg.bms_state.soc = int(soc) + msg.bms_state.current = current_ma msg.bms_state.cycle = 5 msg.bms_state.bq_ntc = [25, 23] msg.bms_state.mcu_ntc = [29, 28] @@ -214,7 +309,7 @@ def create_mock_lowstate(self) -> LowState: 0, 0, 0, - 0, # Remaining cells are 0 + 0, ] # Foot force sensors @@ -228,13 +323,15 @@ def create_mock_lowstate(self) -> LowState: # Wireless remote (all zeros) msg.wireless_remote = [0] * 40 + # Power state (calculated from SoC) + msg.power_v = 24.0 + (soc / 100.0) * 9.6 # ~24V at 0%, ~33.6V at 100% + msg.power_a = current_ma / 1000.0 + # Additional fields msg.bit_flag = 36 msg.adc_reel = 0.0032226562034338713 msg.temperature_ntc1 = 43 msg.temperature_ntc2 = 40 - msg.power_v = 28.67633056640625 - msg.power_a = 1.1279981136322021 msg.fan_frequency = [0, 0, 0, 0] msg.reserve = 0 msg.crc = 1036487475 @@ -245,25 +342,31 @@ def publish_lowstate(self): """ Timer callback to publish LowState messages at regular intervals. """ - msg = self.create_mock_lowstate() + msg = self.create_lowstate_message() self.lowstate_publisher.publish(msg) + self.lf_lowstate_publisher.publish(msg) - # Log every 100 messages (1 second at 100Hz) - if self.tick_counter % 100 == 0: + # Log every 500 messages (5 seconds at 100Hz) + if self.tick_counter % 500 == 0: joint_status = ( "No joint data" if self.latest_joint_state is None else "Joint data OK" ) - self.get_logger().debug( - f"Published LowState - Tick: {msg.tick}, " - f"Battery SOC: {msg.bms_state.soc}%, " - f"IMU Temp: {msg.imu_state.temperature}°C, " - f"Joint Status: {joint_status}" + is_charging = self.get_parameter("is_charging").value + status = ( + "CHARGING" + if is_charging + else ("MOVING" if self.cmd_vel_moving else "IDLE") + ) + self.get_logger().info( + f"[{status}] SoC: {msg.bms_state.soc}%, " + f"Current: {msg.bms_state.current}mA, " + f"Joints: {joint_status}" ) def main(args=None): """ - Main entry point for the lowstate_mock_node. + Main entry point for the go2_lowstate_node. """ rclpy.init(args=args) diff --git a/go2_sdk/go2_sdk/go2_dock.py b/go2_sdk/go2_sdk/go2_dock.py deleted file mode 100644 index 26feab2..0000000 --- a/go2_sdk/go2_sdk/go2_dock.py +++ /dev/null @@ -1,106 +0,0 @@ -#!/usr/bin/env python3 - -import subprocess - -import rclpy -from rclpy.node import Node - -from unitree_go.msg import LowState - - -class Go2AutoChargeMonitor(Node): - def __init__(self): - super().__init__("go2_auto_charge_monitor") - self.get_logger().info("Go2 Auto Charge Monitor node has been started.") - - self.subscription = self.create_subscription( - LowState, "/lf/lowstate", self.listener_callback, 10 - ) - - # State tracking - self.low_battery_threshold = 53.0 # Percentage - self.charging_navigation_triggered = False - self.is_charging = False - self.last_soc = None - - # Timer to periodically check battery status (every 10 seconds) - self.timer = self.create_timer(10.0, self.periodic_check) - - def listener_callback(self, msg): - """Process incoming battery data""" - soc = msg.bms_state.soc # State of charge (battery percentage) - current = msg.bms_state.current # Current in mA (positive = charging) - - # Update charging status - self.is_charging = current > 0 - self.last_soc = soc - - # Only log battery status if navigation hasn't been triggered - if not self.charging_navigation_triggered: - charging_status = "CHARGING" if self.is_charging else "DISCHARGING" - self.get_logger().info( - f"Battery: {soc:.1f}% | Current: {current} mA | Status: {charging_status}" - ) - - # Check if we need to navigate to charger - if ( - soc < self.low_battery_threshold - and not self.is_charging - and not self.charging_navigation_triggered - ): - self.get_logger().warn( - f"Battery low ({soc:.1f}% < {self.low_battery_threshold}%) and not charging!" - ) - self.trigger_charging_navigation() - - # Reset trigger flag if battery is above threshold or charging - if soc >= self.low_battery_threshold or self.is_charging: - if self.charging_navigation_triggered: - self.get_logger().info( - "Battery recovered or charging detected. Resetting trigger." - ) - self.charging_navigation_triggered = False - - def periodic_check(self): - """Periodic status check""" - # Only print periodic updates if navigation hasn't been triggered - if self.last_soc is not None and not self.charging_navigation_triggered: - status = "CHARGING" if self.is_charging else "DISCHARGING" - self.get_logger().info( - f"[Periodic Check] Battery: {self.last_soc:.1f}% | Status: {status}" - ) - - def trigger_charging_navigation(self): - """Launch the navigation to charger script""" - self.get_logger().warn("=" * 60) - self.get_logger().warn("INITIATING AUTOMATIC CHARGING SEQUENCE!") - self.get_logger().warn("=" * 60) - - try: - # Launch the navigation script - subprocess.Popen(["ros2", "run", "my_camera_pkg", "go2_dock"]) - self.charging_navigation_triggered = True - self.get_logger().info( - "Navigation to charger script launched successfully." - ) - - except Exception as e: - self.get_logger().error(f"Failed to launch charging navigation: {str(e)}") - self.charging_navigation_triggered = False - - -def main(args=None): - rclpy.init(args=args) - monitor = Go2AutoChargeMonitor() - - try: - rclpy.spin(monitor) - except KeyboardInterrupt: - monitor.get_logger().info("Keyboard interrupt detected, shutting down...") - finally: - monitor.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/go2_sdk/launch/go2_charge.launch.py b/go2_sdk/launch/go2_charge.launch.py index 974a9d9..f1d102f 100644 --- a/go2_sdk/launch/go2_charge.launch.py +++ b/go2_sdk/launch/go2_charge.launch.py @@ -3,14 +3,14 @@ def generate_launch_description(): - go2_charging_node = Node( - package="my_camera_pkg", - executable="go2_dock", - name="dock_charge_node", + go2_battery_monitor_node = Node( + package="go2_auto_dock", + executable="go2_battery_monitor", + name="go2_battery_monitor", output="screen", ) return LaunchDescription( [ - go2_charging_node, + go2_battery_monitor_node, ] ) From 280466cd974e9cbb5879a5c96c9aa44b94bc0d6d Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 16:00:32 -0800 Subject: [PATCH 12/13] Remove unnecessary comments from Go2 low state publisher and launch file. --- .../go2_gazebo_sim/go2_battery.py | 114 ++++++++++++++++++ .../go2_gazebo_sim/go2_lowstate.py | 6 - .../go2_gazebo_sim/launch/go2_launch.py | 2 - 3 files changed, 114 insertions(+), 8 deletions(-) create mode 100644 go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py diff --git a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py new file mode 100644 index 0000000..0e330ac --- /dev/null +++ b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +"""Simulated battery publisher - auto-detects movement from cmd_vel""" +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import SetParametersResult +from geometry_msgs.msg import Twist +from unitree_go.msg import LowState + + +class MockBatteryPublisher(Node): + def __init__(self): + super().__init__("mock_battery_publisher") + + # Parameters + self.declare_parameter("soc", 80.0) + self.declare_parameter("drain_rate", 0.5) + self.declare_parameter("idle_drain_rate", 0.05) + self.declare_parameter("charge_rate", 2.0) + self.declare_parameter("is_charging", False) + self.declare_parameter("publish_rate", 1.0) + self.declare_parameter("velocity_threshold", 0.01) + self.declare_parameter("cmd_vel_timeout", 0.5) + + # State + self.cmd_vel_moving = False + self.last_cmd_vel_time = self.get_clock().now() + + # Parameter callback + self.add_on_set_parameters_callback(self.parameter_callback) + + # Subscribers + self.create_subscription(Twist, "/cmd_vel", self.cmd_vel_callback, 10) + + # Publisher + self.pub = self.create_publisher(LowState, "/lf/lowstate", 10) + + # Timer + publish_rate = self.get_parameter("publish_rate").value + self.timer = self.create_timer(1.0 / publish_rate, self.update_and_publish) + + self.get_logger().info("Mock Battery Publisher Started") + self.get_logger().info(" Output: /lf/lowstate") + self.get_logger().info(" Listens: /cmd_vel") + + def parameter_callback(self, params): + for param in params: + if param.name == "is_charging": + status = "STARTED" if param.value else "STOPPED" + self.get_logger().info(f"Charging {status}") + elif param.name == "soc": + self.get_logger().info(f"SoC manually set to {param.value:.1f}%") + + return SetParametersResult(successful=True) + + def cmd_vel_callback(self, msg: Twist): + threshold = self.get_parameter("velocity_threshold").value + linear_speed = (msg.linear.x**2 + msg.linear.y**2) ** 0.5 + angular_speed = abs(msg.angular.z) + + self.cmd_vel_moving = linear_speed > threshold or angular_speed > threshold + self.last_cmd_vel_time = self.get_clock().now() + + def update_and_publish(self): + soc = self.get_parameter("soc").value + is_charging = self.get_parameter("is_charging").value + timeout = self.get_parameter("cmd_vel_timeout").value + + # Check if moving + time_since_cmd = (self.get_clock().now() - self.last_cmd_vel_time).nanoseconds / 1e9 + is_moving = self.cmd_vel_moving and time_since_cmd < timeout + + if is_charging: + is_moving = False + + # Update SoC + if is_charging: + soc = min(100.0, soc + self.get_parameter("charge_rate").value) + current_ma = 3000 + elif is_moving: + soc = max(0.0, soc - self.get_parameter("drain_rate").value) + current_ma = -5000 + else: + soc = max(0.0, soc - self.get_parameter("idle_drain_rate").value) + current_ma = -500 + + self.set_parameters([rclpy.parameter.Parameter("soc", value=soc)]) + + # Publish + msg = LowState() + msg.bms_state.soc = int(soc) + msg.bms_state.current = current_ma + msg.power_v = 24.0 + (soc / 100.0) * 9.6 + msg.power_a = current_ma / 1000.0 + + self.pub.publish(msg) + + status = "CHARGING" if is_charging else ("MOVING" if is_moving else "IDLE") + self.get_logger().info(f"{status} | SoC: {soc:.1f}% | Current: {current_ma}mA") + + +def main(args=None): + rclpy.init(args=args) + node = MockBatteryPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py index c805e18..75e2ee7 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py +++ b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_lowstate.py @@ -1,10 +1,4 @@ #!/usr/bin/env python3 -""" -A ROS2 node that publishes mock LowState messages for Unitree Go2 simulation. -This provides simulated robot state data including IMU, motor, and battery information. -Battery simulation auto-detects movement from cmd_vel for realistic drain behavior. -""" - import rclpy from geometry_msgs.msg import Twist from rcl_interfaces.msg import SetParametersResult diff --git a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py index c809566..c99d41f 100644 --- a/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py +++ b/go2_gazebo_sim/go2_gazebo_sim/launch/go2_launch.py @@ -35,8 +35,6 @@ def generate_launch_description(): default_model_path = os.path.join(go2_description, "urdf/unitree_go2_robot.xacro") default_world_path = os.path.join(go2_description, "worlds/home_world.sdf") aruco_model_path = os.path.join(go2_description, "models/aruco_marker/model.sdf") - - # Add go2_description/models to GZ_SIM_RESOURCE_PATH go2_description_models = os.path.join(go2_description, "models") # Ensure GZ_SIM_RESOURCE_PATH includes the models directory From fc887d2ca1a42a49762ea5cec8b99241407d7fd0 Mon Sep 17 00:00:00 2001 From: jerinpeter Date: Fri, 12 Dec 2025 16:07:15 -0800 Subject: [PATCH 13/13] remove mock battery publisher for Go2 Gazebo simulation and merged the functionality with lowstate --- .../go2_gazebo_sim/go2_battery.py | 114 ------------------ 1 file changed, 114 deletions(-) delete mode 100644 go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py diff --git a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py b/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py deleted file mode 100644 index 0e330ac..0000000 --- a/go2_gazebo_sim/go2_gazebo_sim/go2_gazebo_sim/go2_battery.py +++ /dev/null @@ -1,114 +0,0 @@ -#!/usr/bin/env python3 -"""Simulated battery publisher - auto-detects movement from cmd_vel""" -import rclpy -from rclpy.node import Node -from rcl_interfaces.msg import SetParametersResult -from geometry_msgs.msg import Twist -from unitree_go.msg import LowState - - -class MockBatteryPublisher(Node): - def __init__(self): - super().__init__("mock_battery_publisher") - - # Parameters - self.declare_parameter("soc", 80.0) - self.declare_parameter("drain_rate", 0.5) - self.declare_parameter("idle_drain_rate", 0.05) - self.declare_parameter("charge_rate", 2.0) - self.declare_parameter("is_charging", False) - self.declare_parameter("publish_rate", 1.0) - self.declare_parameter("velocity_threshold", 0.01) - self.declare_parameter("cmd_vel_timeout", 0.5) - - # State - self.cmd_vel_moving = False - self.last_cmd_vel_time = self.get_clock().now() - - # Parameter callback - self.add_on_set_parameters_callback(self.parameter_callback) - - # Subscribers - self.create_subscription(Twist, "/cmd_vel", self.cmd_vel_callback, 10) - - # Publisher - self.pub = self.create_publisher(LowState, "/lf/lowstate", 10) - - # Timer - publish_rate = self.get_parameter("publish_rate").value - self.timer = self.create_timer(1.0 / publish_rate, self.update_and_publish) - - self.get_logger().info("Mock Battery Publisher Started") - self.get_logger().info(" Output: /lf/lowstate") - self.get_logger().info(" Listens: /cmd_vel") - - def parameter_callback(self, params): - for param in params: - if param.name == "is_charging": - status = "STARTED" if param.value else "STOPPED" - self.get_logger().info(f"Charging {status}") - elif param.name == "soc": - self.get_logger().info(f"SoC manually set to {param.value:.1f}%") - - return SetParametersResult(successful=True) - - def cmd_vel_callback(self, msg: Twist): - threshold = self.get_parameter("velocity_threshold").value - linear_speed = (msg.linear.x**2 + msg.linear.y**2) ** 0.5 - angular_speed = abs(msg.angular.z) - - self.cmd_vel_moving = linear_speed > threshold or angular_speed > threshold - self.last_cmd_vel_time = self.get_clock().now() - - def update_and_publish(self): - soc = self.get_parameter("soc").value - is_charging = self.get_parameter("is_charging").value - timeout = self.get_parameter("cmd_vel_timeout").value - - # Check if moving - time_since_cmd = (self.get_clock().now() - self.last_cmd_vel_time).nanoseconds / 1e9 - is_moving = self.cmd_vel_moving and time_since_cmd < timeout - - if is_charging: - is_moving = False - - # Update SoC - if is_charging: - soc = min(100.0, soc + self.get_parameter("charge_rate").value) - current_ma = 3000 - elif is_moving: - soc = max(0.0, soc - self.get_parameter("drain_rate").value) - current_ma = -5000 - else: - soc = max(0.0, soc - self.get_parameter("idle_drain_rate").value) - current_ma = -500 - - self.set_parameters([rclpy.parameter.Parameter("soc", value=soc)]) - - # Publish - msg = LowState() - msg.bms_state.soc = int(soc) - msg.bms_state.current = current_ma - msg.power_v = 24.0 + (soc / 100.0) * 9.6 - msg.power_a = current_ma / 1000.0 - - self.pub.publish(msg) - - status = "CHARGING" if is_charging else ("MOVING" if is_moving else "IDLE") - self.get_logger().info(f"{status} | SoC: {soc:.1f}% | Current: {current_ma}mA") - - -def main(args=None): - rclpy.init(args=args) - node = MockBatteryPublisher() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() \ No newline at end of file