From 620ee000b6fbaa5e961003327a16450755898ed5 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Fri, 24 Feb 2023 15:26:06 +0000 Subject: [PATCH 01/13] [xslocobot_nav] Add sample maps --- .../maps/interbotix_map.pgm | Bin 0 -> 112127 bytes .../maps/interbotix_map.yaml | 7 +++++++ 2 files changed, 7 insertions(+) create mode 100644 interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm create mode 100644 interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm new file mode 100644 index 0000000000000000000000000000000000000000..01171615445f576640e35c530bb348b3909f1edd GIT binary patch literal 112127 zcmeI5ZIb-Z35PUZ9JU~_1R91`g)uim+`4NPXgBmU>C8erJd8NB3uZ}p1GiNI* zc&$ z?Vvv|MO(h9B=q=IxPgk=bl9p>@t_&T;xK0R2saKV>d>ib2$GURggK$ajtm=@^9>~# z>XrYXRNc;vDBaVwp^)~&IL^ldvLMnRcHvk0M-6TH&K=9iq=9dlo|m$(!7#k}in>R! z=Yx{&9gR@~m2gMn2rv~Kp~C&5X}Se_(n>Ci#eCi`9KMaO#PUMTsPAGu*~U`D1KB=p zW6cJRPWP+$kvp0g2vURm?tkSD%Q$lFZaCI4>2@u@n1g5nv7Ow|A4S$bAi(kX$w#q( zKx{_)#T=jklt)l73eg{R`~v_ek_3hXg8ue53`xI~@(kTRj>Eo>_Ol{6vv4q+{Cq?h zn2NpMX~rX~vl1el^wg?GY$-o;1|ph%lRFfVr4Q{UOf zHCT2G-4-0i36WXXaA-6AutSGIq>ir+Lqfx{pl*zs)1c!%4hDuBCY`Sahx1pLS4t>E z0$fTkgfktZ<^X~fwn-#?5o6_CmI2`+DX^{Zn zqKD$yNacXMR2uc+_pRgN0U5*V?DoLm0>anu-tfRx3ZB#?Nve09{-V6(RgaQyU`XUk zMC~e4h?&yeW4zY z0gZ;xkE5ex<$$-~lWlfh($1Va5(tWb0NB7_dEjcc=(9QW&rA=&WAJ;LYg-t`KD9Vf zRt`xlaPfi=ZJk3l(`&sl1X&!y3K0NoVi=Z}-0*A}^O!+@skuy}CO0AXQ%-}_~ZnXp?-QP(o$HMccXzZ4CO#}(BRg*1Ur(=z%ccDNExP&=*WkIi-kiB7vv@C z@=QF$AJT9}LP%O7|8FT^v&H0Kj3e$3x6XjATG!NKLbpZ7FrDXU4$kNTyf=i9tyq zVxW@E8MIarG03DXk#_5Crf+wJOcZia1BQ-x;1uL6Q5e9%kYuT~-aeiK2pBP(&&4SQ z(7?!erOmuPXcC{!K``Kzx0Yx){}{J}p*Js`pBtD2ecOn<#4|F%;DP9<&yG$`z8r%@ zf&e;wSk@Ihh}fa#|2bdNSsv{O)>`NmZ2&D*HK* z5C{m-najE4)nmhgy5~ZS4=UN5!C#hCrp@NC=V!|~kAZ0vt3y#CzK?bqMi}EE|Le-5 zO={83x3Ntje^s()2&{D-1F7y}C7F99*ob&g!C;RE2A5j(!Ow9L3qVlCun`R8{D8@) zFkoufCKWwwBoM}!luS@t(PnzBZ->BRP_Aub7{I|}@O$&~!&hPGYT@UK|;vs>+Q(vjR6AS<}C>h6SFn}NJzev%@fsFjBc;IkQLLUVKHSS#=JO+j&ERbdlc;zuJAz`?-J~cXOyVq$7 z1WZhYq2-{{X{Wq+E(hlI0pXLH+G$b;1?!_vF%42j4L;F!WV z4upVF;K|U;Av3i1-a$N-67KPVSu_F}9H4(>Ku$Fh5ck{XZPhOt4I2!= zEa0N`ehC(*@zDKD12ax;9UK?}M1ZiOhm=L=gV$k`FHkVwx8@cOGwB424h}pS;fn*2 zyS0CO)@g|Y_SA=gTO&?zKrF@WtVt(iFD~iefEW-bXeU3>r^C`1rxPmVn#~zXNjfi$ zF{zNaslsM@(j_Rk>7*vrApqB4gwvXWokN&NV$%qd5`|2~(;NUn0Px<-VvN(81kG?m zx8%C>VBpj+iJvq+$$|ZkUIZqs(O?oRF*hx028d0YGq_b!)5pLtwPQ%xW_lcwL|`Y_ z5#>!Iie9G+3S^tj8E{bdt-U5ib?haZ>7jp*0j`_~1_;pk01-?<0Y=bfa|WNssTnlX zBsQr1#b&zi*J}?qlq=xUwskozvM1I^(dLYNF3M5iOddHkym5_to9V7oOeFkZKH<-q zt&2n9oPe=8lP}X_L$ok)zrtp^^yRc?{gD$+Z38)k=&(6cd{1DYN&BK#&e}}p4qOmO zK%8QLi%vQnG_Uw#`+S-VKqx19>7+?DOU!jAI7lv%*V7oza>yKvSjZCdI-RA|595G% zpow%kBMG~qPM^77LOkfZjG-oF6Xxj991k)Bg~a)2IGvX&{)k05K{=-VmIoa4;`T`# z;4}wBXE+!Ht4JTKA_Ow)42REMAAlj7Nhbnh?Brt{SaWUhpzR)OQk*L-C39Xvr{uNv z;KWg<(`g)_Ocu!Vbc@s6hCI&}h)$E5N3JzAatQ99P+nis<8-isge0e!0rtjtAP)K& zqCJ>uQg+oDIl$WiNr{vj;Im@jD7SaZ@c@Ql>zNu|5a?nX2Qc9I7i>RWXcw&NqTF%v zsbB&-Q*B+V{A3Ok#*87k$p|VK3mub>8HluQP(I*tJ6__z3q-WL!=xH$Q=Kgy&~>!C zt8~_B8dMyV(Yc%$V&kAmoHa@8>3g+t7_>G(w=+(A1Tq(L1Wd!iffxAX4W=_D^{F4P zws9DmQj#?n<@dB5k5^oxyy4(<=)pbgg&*pAu-gX%sP?GTXR+q)&zKXrYuDZSsi zeeyX5jAOq~I&0#P&8l{jcsdMyY0pdg9!Vb^%2PJ5I$icqt+8;B)h>EsHEAS>b`Dt( zmnyJB;#KsmZ02eVPH*;6Ibq^}+odEX3?_|4zOSU9+F z2;Fa+lp(y}uq__wZvet`aveWE^ud=Dphmjsqx4MLkXvmF#(&+AKU_lxc=L`nvB0a3 zD&_z)l0Q`2tP#19C8noyfCl5YO~Lwx+Uzzz;W>n~M6B~R_Z{=RzHd0=NF)pepJ{6z z0dWQfi$k+ymo&ueN5+fWiib;tKIoL<;nVJ04?OzJ)0FM-MVn+>@i2oV$3sE^_hd8d zkTIXVPG70Z%1c+O+uk>3KZQe@npQlt%QvhgaY*i2k)Y=0C9GF(+=N4V>pD{ienxsD z2KRo+Q#ia50XvO&pr2%$84qvf@TBLsH*?U@Fn67PGlvxqPoU5DiOK4);$aRIdY;CX zJjbnddIkx;UrIlInED(yLxE?E2@dMEb#a&i23bgj!{V?wOi|z&gZj@O;zGUF=^13e zAu`}wr+2>LIYazp%x8TLlo5%=Lfm?vN+7)fG^IZ+u|^X1ASL8 z#ju_qW?-Ptx^KNn>%C#hb$Z3ao?cg-Bh~R&>ymA8xJv82emNf0gWTeNW*pN@#NPG6_?cF&~x%1hhw z+;#@xpl(~SK+N54^ZiK!RXE`7SG?WfHiCkR@TM$EILj^gg)5|r+xiPpj|%*_Jzqvm zQ9+yUHnyV5C&_c0ElvzCdVSBr@#l6U6hiu8&}NJwcV`%#Y0R+|Uuy60nJeb9+%#Y3*q9mk|PKLy7%VsW@b_k4_XzmzL7=eS5q zc!Owdw<|#Lpl_EJhuaxoK*pd*to30A2vS{9z?;uA?0EZ_6c75gb>*cgQ=+mwtaz9} zf;eP(Y27bPz(DIXC8GEchti6N2_UdMWDCUNFoi?DPOtl=2^jGFkf)^;4--Jp?9Vu? z^CxjwACGyy~9erf9S!;AA$zD_R=lj0$+3{qO_!vqk|S|8})W$N-Y0e~B_Bp=Oi$fX>D=$q+%TIcq z#&mdZUedt;DW*&a8gy|`EEb0e6S2%ov)Y*mx?2K`5)zLZ(hX`+tDwIxv|QKF zwXw#;kPBa zn;NxrL~Z=&i-T8!IE)l{acIgfr|(+_v+}dxrSU-ju7g(~?3?%v#<23zNe+$arn#A) z1uu<aPeKf#UUgH_xvE!G@0;K5tH+#qVEg{@DoR0>vYfp z>w_0csavg7Ep`ufE)Qd`bvluvPGMK$vnL&!FnD7+Jv-d}*EKp+xo_QGr~AA}!oe45 z&&vblC7+sJk(a#xQP2tpyw6uQOls=I*k@Y_z0mI)Inb=jw>pQiU0IX&baY(p0TgiJ zUGdPyA;)cFpFB zztAf~;JoCmE2fV&_w$YlhrFlSm5Vr}LhAupebHY+gUa4C?8)xzg3mtOTJkhr#Uai)rOmY*G;`!5&P50T`XfJ7O15J9C~H6PAa^MGs+_r&gBnT>90~^E zctq1)@cBb@R*7>dhk7`0C`1m$^6=g3!;Kug{C*$6yIAmH5LiUpzJ8rfTn)0IupbYF zlIq%{gW~D{z!TuTS|7gsektLg0gkQ*tOZEQyD-`XDc%>5;OkyKo?SheIhS`MCwX;3wHKyR#lX?=YKKiKoh! z!+6-nuzg025Dq*Ay3Q9Fa$?%o-?yf|24PUx;^0O0`}88ItDOn0AauT=#siQ&9~R-D z;yCZ*FvkKKx%cVG(RNuHhZtB!JxKshg7DosWV$~|$p+LrghAeCDuw#v3QQ-f4 C;eA*D literal 0 HcmV?d00001 diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml new file mode 100644 index 0000000..ff3eddf --- /dev/null +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml @@ -0,0 +1,7 @@ +image: interbotix_map.pgm +mode: trinary +resolution: 0.05 +origin: [-9.38, -5.22, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 From 6b4b1efd863ef28caa29881514fd5ccae91dda0b Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Fri, 24 Feb 2023 15:28:00 +0000 Subject: [PATCH 02/13] [xslocobot_sim] Update navigation in RViZ --- .../rviz/xslocobot_gz_classic.rviz | 146 ++++++++++++++---- 1 file changed, 116 insertions(+), 30 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz b/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz index b866b80..c57e597 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz @@ -9,8 +9,8 @@ Panels: Visualization Manager: Class: "" Displays: - - Alpha: 0.699999988079071 - Cell Size: 0.25 + - Alpha: 0.3 + Cell Size: 0.1 Class: rviz_default_plugins/Grid Color: 255; 255; 255 Enabled: true @@ -24,7 +24,7 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 10 + Plane Cell Count: 300 Reference Frame: Value: true - Alpha: 1 @@ -472,50 +472,96 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: false + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Timestamp: false + Value: true + - Alpha: 5 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: false + Max Arrow Length: 0.3 + Min Arrow Length: 0.02 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true - Class: rviz_common/Group Displays: - - Alpha: 0.699999988079071 + - Alpha: 0.5 Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true + Color Scheme: costmap + Draw Behind: false Enabled: true - Name: Map + Name: Global Costmap Topic: Depth: 5 Durability Policy: Transient Local - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates + Value: /global_costmap/costmap Use Timestamp: false Value: true - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 Enabled: true - Name: Global Costmap + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/costmap - Update Topic: + Value: /plan + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /global_costmap/costmap_updates - Use Timestamp: false - Value: true + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: costmap @@ -524,7 +570,7 @@ Visualization Manager: Name: Local Costmap Topic: Depth: 5 - Durability Policy: Volatile + Durability Policy: Transient Local Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable @@ -537,11 +583,51 @@ Visualization Manager: Value: /local_costmap/costmap_updates Use Timestamp: false Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false - Alpha: 1 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 Enabled: true - Name: Robot Footprint + Name: Polygon Topic: Depth: 5 Durability Policy: Volatile @@ -551,11 +637,11 @@ Visualization Manager: Value: /local_costmap/published_footprint Value: true Enabled: true - Name: Navigation + Name: Local Planner Enabled: true Global Options: Background Color: 200; 200; 200 - Fixed Frame: map + Fixed Frame: locobot/odom Frame Rate: 30 Name: root Tools: @@ -623,7 +709,7 @@ Window Geometry: collapsed: false Height: 923 Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Image: collapsed: false QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000221000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000002420000013c0000000000000000fb0000000a0049006d006100670065010000023d000001410000002800fffffffb0000000c00430061006d0065007200610100000289000000f50000000000000000fb0000000a0049006d00610067006501000002b6000000c800000000000000000000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 From 7bfedce0ed7f5efabfd32720858a2e81647e3644 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Fri, 24 Feb 2023 15:29:59 +0000 Subject: [PATCH 03/13] [xslocobot_nav] Fix nav2 functionalities amcl, map_server, use_sim_time parameter substitution, behavior server in humble, static costmap layer --- .../config/nav2_params.yaml | 292 +++++++++++++----- .../launch/xslocobot_nav2_bringup.launch.py | 38 +-- .../launch/xslocobot_slam_toolbox.launch.py | 5 +- 3 files changed, 245 insertions(+), 90 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml index 5fb762c..1c57066 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/config/nav2_params.yaml @@ -16,6 +16,9 @@ bt_navigator: # for details on this file. New to Galactic after NavigateThroughPoses was added. # default_nav_through_poses_bt_xml: CONFIGURED IN LAUNCH FILE + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Reference frame. (default: map) global_frame: map @@ -52,53 +55,85 @@ bt_navigator: groot_zmq_server_port: 1667 # List of behavior tree node shared libraries. (default: - # nav2_compute_path_to_pose_action_bt_node, - # nav2_follow_path_action_bt_node, - # nav2_back_up_action_bt_node, - # nav2_spin_action_bt_node, - # nav2_wait_action_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_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_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_distance_traveled_condition_bt_node, - # nav2_single_trigger_bt_node) + # - 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 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_back_up_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_goal_updated_controller_bt_node - nav2_is_battery_low_condition_bt_node # TODO(lsinterbotix): configure to use mobile_base battery - nav2_navigate_through_poses_action_bt_node - nav2_navigate_to_pose_action_bt_node @@ -106,12 +141,32 @@ bt_navigator: - 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 + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False # The Controller Server implements the server for handling the controller requests for the stack # and host a map of plugin implementations. It will take in path and plugin names for controller, # progress checker and goal checker to use and call the appropriate plugins. controller_server: ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Frequency to run controller (Hz). (default 20.0) controller_frequency: 20.0 @@ -434,6 +489,9 @@ controller_server: local_costmap: local_costmap: ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Whether to send full costmap every update, rather than updates. (default: false) always_send_full_costmap: True @@ -510,7 +568,7 @@ local_costmap: # How long to store messages in a buffer to add to costmap before removing them (s). # (default: 0.0) - observation_persistence: 0.0 + observation_persistence: 0.2 # Expected rate to get new data from sensor. (default: 0.0) expected_update_rate: 0.0 @@ -546,19 +604,15 @@ local_costmap: # Minimum range to raytrace clear obstacles from costmap. (default: 0.0) raytrace_min_range: 0.0 - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - # https://navigation.ros.org/configuration/packages/costmap-plugins/static.html - - # QoS settings for map topic. (default: True) - map_subscribe_transient_local: True - # The Costmap 2D package implements a 2D grid-based costmap for environmental representations and a # number of sensor processing plugins. It is used in the planner and controller servers for # creating the space to check for collisions or higher cost areas to negotiate around. global_costmap: global_costmap: ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Whether to send full costmap every update, rather than updates. (default: false) always_send_full_costmap: True @@ -631,13 +685,25 @@ global_costmap: # loaded in the namespace. plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - # List of mapped costmap filter names for parameter namespaces and names. Note: Costmap - # filters are also loadable plugins just as ordinary costmap layers. This separation is made - # to avoid plugin and filter interference and places these filters on top of the combined - # layered costmap. As with plugins, each costmap filter namespace defined in this list needs - # to have a plugin parameter defining the type of filter plugin to be loaded in the - # namespace. - # filters: + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + # https://navigation.ros.org/configuration/packages/costmap-plugins/static.html + + # Whether the static_layer plugin is enabled. (default: True) + enabled: True + + # Subscribe to static map updates after receiving first. (default: False) + subscribe_to_updates: False + + # QoS settings for map topic. (default: True) + map_subscribe_transient_local: True + + # TF tolerance. (default: 0.0) + transform_tolerance: 1.0 + + # Map topic to subscribe to. If left empty the map topic will default to the global + # map_topic parameter in costmap_2d_ros. (default: "") + map_topic: "/map" obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" @@ -667,7 +733,7 @@ global_costmap: # How long to store messages in a buffer to add to costmap before removing them (s). # (default: 0.0) - observation_persistence: 0.0 + observation_persistence: 0.2 # Expected rate to get new data from sensor. (default: 0.0) expected_update_rate: 0.0 @@ -702,27 +768,7 @@ global_costmap: # Minimum range to raytrace clear obstacles from costmap. (default: 0.0) raytrace_min_range: 0.0 - - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - # https://navigation.ros.org/configuration/packages/costmap-plugins/static.html - - # Whether the static_layer plugin is enabled. (default: True) - enabled: True - - # Subscribe to static map updates after receiving first. (default: False) - subscribe_to_updates: False - - # QoS settings for map topic. (default: True) - map_subscribe_transient_local: True - - # TF tolerance. (default: 0.0) - transform_tolerance: 0.0 - - # Map topic to subscribe to. If left empty the map topic will default to the global - # map_topic parameter in costmap_2d_ros. (default: "") - map_topic: "" - + inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" # https://navigation.ros.org/configuration/packages/costmap-plugins/inflation.html @@ -749,6 +795,9 @@ global_costmap: map_saver: ros__parameters: # https://navigation.ros.org/configuration/packages/configuring-map-server.html + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False # Timeout to attempt saving the map (seconds). (default: 2) save_map_timeout: 5.0 @@ -769,7 +818,11 @@ planner_server: ros__parameters: # Expected planner frequency. If the current frequency is less than the expected frequency, # display the warning message. - expected_planner_frequency: 20.0 + expected_planner_frequency: 5.0 + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner/NavfnPlanner" @@ -784,7 +837,6 @@ planner_server: # points of the path. (default: false) use_final_approach_orientation: false -# TODO (lsinterbotix): Update to behavior_server on Humble # The Behavior Server implements the server for handling recovery behavior requests and hosting a # vector of plugins implementing various C++ behaviors. It is also possible to implement # independent behavior servers for each custom behavior, but this server will allow multiple @@ -797,20 +849,32 @@ behavior_server: ros__parameters: # https://navigation.ros.org/configuration/packages/configuring-behavior-server.html?highlight=recoveries_server - # Raw costmap topic for collision checking. (default: local_costmap/costmap_raw) - costmap_topic: local_costmap/costmap_raw + # Raw local costmap topic for collision checking. (default: local_costmap/costmap_raw) + local_costmap_topic: local_costmap/costmap_raw + + # Topic for footprint in the local costmap frame. (default: local_costmap/published_footprint) + local_footprint_topic: local_costmap/published_footprint - # Topic for footprint in the costmap frame. (default: local_costmap/published_footprint) - footprint_topic: local_costmap/published_footprint + # Raw global costmap topic for collision checking. (default: global_costmap/costmap_raw) + global_costmap_topic: global_costmap/costmap_raw + + # Topic for footprint in the global costmap frame. (default: global_costmap/published_footprint) + global_footprint_topic: global_costmap/published_footprint # Frequency to run behavior plugins. (default: 10.0) cycle_frequency: 10.0 # TF transform tolerance. (default: 0.1) - transform_tolerance: 0.1 + transform_tolerance: 0.3 + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False - # Reference frame. (default: odom) - global_frame: locobot/odom + # Locl reference frame. (default: odom) + local_frame: locobot/odom + + # Global reference frame. (default: map) + global_frame: map # Robot base frame. (default: base_link) robot_base_frame: locobot/base_link @@ -818,7 +882,7 @@ behavior_server: # List of plugin names to use, also matches action server names. Note: Each plugin namespace # defined in this list needs to have a plugin parameter defining the type of plugin to be # loaded in the namespace. (default: {“spin”, “back_up”, “drive_on_heading”, “wait”}) - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: plugin: "nav2_behaviors/Spin" @@ -851,6 +915,9 @@ waypoint_follower: ros__parameters: # https://navigation.ros.org/configuration/packages/configuring-waypoint-follower.html + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + # Whether to fail action task if a single waypoint fails. If false, will continue to next # waypoint. (default: True) stop_on_failure: True @@ -873,3 +940,88 @@ waypoint_follower: # Amount of time in milliseconds, for robot to sleep/wait after each waypoint is reached. If # zero, robot will directly continue to next waypoint. (default: 0) waypoint_pause_duration: 200 + +# The Map Server implements the server for handling the map load requests for the stack and host a map topic. +# It also implements a map saver server which will run in the background and save maps based on service requests. +# There exists a map saver CLI similar to ROS 1 as well for a single map save. +map_server: + ros__parameters: + # https://navigation.ros.org/configuration/packages/configuring-map-server.html + + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + + # Path to map yaml file. + yaml_filename: "" + + # Topic to publish loaded map to. (default: "map") + topic_name: "map" + + # Frame to publish loaded map in. (default: "map") + frame_id: "map" + +map_saver: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + + # Timeout to attempt saving the map (seconds). (default: 2.0) + save_map_timeout: 5.0 + + # Free space maximum probability threshold value for occupancy grid. (default: 0.25) + free_thresh_default: 0.25 + + # Occupied space minimum probability threshhold value for occupancy grid. (default: 0.65) + occupied_thresh_default: 0.65 + +# For localization mode with AMCL (without using slam_toolbox) +amcl: + ros__parameters: + # Whether to use simulation time. This paremeter will always be rewritten from the launch file. + use_sim_time: False + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "locobot/base_footprint" + 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: 16.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "locobot/odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 0.3 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: locobot/scan + map_topic: map + set_initial_pose: true + always_reset_initial_pose: false + first_map_only: false + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 \ No newline at end of file diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index 1b44108..8284f16 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -46,7 +46,6 @@ from launch.substitutions import ( PathJoinSubstitution, LaunchConfiguration, - TextSubstitution, ) from launch_ros.actions import ( Node, @@ -83,7 +82,7 @@ def launch_setup(context, *args, **kwargs): ] lifecycle_nodes_localization = [ - 'map_server' + 'map_server', 'amcl' ] @@ -97,14 +96,14 @@ def launch_setup(context, *args, **kwargs): # https://github.com/ros/robot_state_publisher/pull/30 tf_remappings = [ ('/tf', 'tf'), - ('/tf_static', 'tf_static' - ) + ('/tf_static', 'tf_static') ] # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time_launch_arg, 'autostart': autostart_launch_arg, + 'yaml_filename': map_yaml_file_launch_arg, } configured_params = RewrittenYaml( @@ -185,21 +184,20 @@ def launch_setup(context, *args, **kwargs): actions=[ SetParameter('use_sim_time', use_sim_time_launch_arg), Node( - condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationNotEquals('slam_mode', 'localization_amcl'), package='nav2_map_server', executable='map_saver_server', + name='map_saver_server', output='screen', respawn=use_respawn_launch_arg, respawn_delay=2.0, arguments=[ '--ros-args', '--log-level', log_level_launch_arg ], - parameters=[ - configured_params - ], + parameters=[configured_params], ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization_amcl'), package='nav2_map_server', executable='map_server', name='map_server', @@ -209,14 +207,11 @@ def launch_setup(context, *args, **kwargs): arguments=[ '--ros-args', '--log-level', log_level_launch_arg ], - parameters=[ - configured_params, - {'yaml_filename': map_yaml_file_launch_arg}, - ], + parameters=[configured_params], remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization_amcl'), package='nav2_amcl', executable='amcl', name='amcl', @@ -228,7 +223,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationNotEquals('slam_mode', 'localization_amcl'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', @@ -237,12 +232,13 @@ def launch_setup(context, *args, **kwargs): '--ros-args', '--log-level', log_level_launch_arg ], parameters=[ + {'use_sim_time': use_sim_time_launch_arg}, {'autostart': autostart_launch_arg}, {'node_names': lifecycle_nodes_slam}, ] ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization_amcl'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', @@ -251,6 +247,7 @@ def launch_setup(context, *args, **kwargs): '--ros-args', '--log-level', log_level_launch_arg ], parameters=[ + {'use_sim_time': use_sim_time_launch_arg}, {'autostart': autostart_launch_arg}, {'node_names': lifecycle_nodes_localization}, ] @@ -330,18 +327,21 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - 'slam_toolbox_mode', + 'slam_mode', default_value='online_async', choices=( # 'lifelong', 'localization', + 'localization_amcl', # 'offline', 'online_async', 'online_sync' ), description=( - "the node to launch the SLAM in using the slam_toolbox. Currently only " + "the mode to launch the SLAM in using the slam_toolbox. Currently only " "'localization', 'online_sync', and 'online_async' modes are supported." + "'localization' mode takes a pose graph map defined in the slam_toolbox_localization.yaml" + "'localization_amcl' mode instead uses the amcl package to localize, given a map yaml file." ), ) ) @@ -355,7 +355,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( 'log_level', - default_value='info', + default_value='warn', choices=('debug', 'info', 'warn', 'error', 'fatal'), description='set the logging level of the Nav2 nodes.' ) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py index 5ba6ab9..0c1b5c1 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py @@ -170,7 +170,7 @@ def launch_setup(context, *args, **kwargs): 'autostart': 'true', 'params_file': LaunchConfiguration('nav2_params_file'), 'use_slam_toolbox': 'true', - 'slam_toolbox_mode': slam_mode_launch_arg, + 'slam_mode': slam_mode_launch_arg, 'map': map_yaml_file_launch_arg, }.items(), ) @@ -279,6 +279,7 @@ def generate_launch_description(): choices=( # 'lifelong', 'localization', + 'localization_amcl', # 'offline', 'online_async', 'online_sync' @@ -286,6 +287,8 @@ def generate_launch_description(): description=( "the mode to launch the SLAM in using the slam_toolbox. Currently only " "'localization', 'online_sync', and 'online_async' modes are supported." + "'localization' mode takes a pose graph map defined in the slam_toolbox_localization.yaml" + "'localization_amcl' mode instead uses the amcl package to localize, given a map yaml file." ), ) ) From 37102e395e0fb0e792619c3d3cb48ba391fddd48 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Fri, 3 Mar 2023 18:12:17 +0000 Subject: [PATCH 04/13] [xslocobot_nav] Minor hotfix Renaming slam_mode launch argument to slam_toolbox_mode so it will not interfere when launching rtabmap. Log level info. --- .../launch/xslocobot_nav2_bringup.launch.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index 8284f16..b7a4526 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -184,7 +184,7 @@ def launch_setup(context, *args, **kwargs): actions=[ SetParameter('use_sim_time', use_sim_time_launch_arg), Node( - condition=LaunchConfigurationNotEquals('slam_mode', 'localization_amcl'), + condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization_amcl'), package='nav2_map_server', executable='map_saver_server', name='map_saver_server', @@ -197,7 +197,7 @@ def launch_setup(context, *args, **kwargs): parameters=[configured_params], ), Node( - condition=LaunchConfigurationEquals('slam_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization_amcl'), package='nav2_map_server', executable='map_server', name='map_server', @@ -211,7 +211,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization_amcl'), package='nav2_amcl', executable='amcl', name='amcl', @@ -223,7 +223,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationNotEquals('slam_mode', 'localization_amcl'), + condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization_amcl'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', @@ -238,7 +238,7 @@ def launch_setup(context, *args, **kwargs): ] ), Node( - condition=LaunchConfigurationEquals('slam_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization_amcl'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', @@ -286,7 +286,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( 'cmd_vel_topic', - default_value=(LaunchConfiguration('robot_name'), '/mobile_base/cmd_vel'), + default_value=(LaunchConfiguration('robot_name'), '/diffdrive_controller/cmd_vel_unstamped'), description="topic to remap /cmd_vel to." ) ) @@ -327,7 +327,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - 'slam_mode', + 'slam_toolbox_mode', default_value='online_async', choices=( # 'lifelong', @@ -355,7 +355,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( 'log_level', - default_value='warn', + default_value='info', choices=('debug', 'info', 'warn', 'error', 'fatal'), description='set the logging level of the Nav2 nodes.' ) From b1f16266d928300d3b51fba24991e9b30184e175 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Fri, 3 Mar 2023 20:04:06 +0000 Subject: [PATCH 05/13] [xslocobot_nav] Improve map quality --- .../maps/interbotix_map.pgm | Bin 112127 -> 106423 bytes .../maps/interbotix_map.yaml | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.pgm index 01171615445f576640e35c530bb348b3909f1edd..238a7f7a6d44ee3f2d35c90f1bf46aa60fd7b0ce 100644 GIT binary patch literal 106423 zcmeI5ZMNbz42JnVtLPeL&a_=dH_n#ssSP)SLjscg^+I@ZrWeOjq~!4%#RMAu`~BnB zKY#!A^WR@Te*XUbGajKoM2c*AaOspfIk7gmU|O;xL09suQTYq z0(`otel^Mc3%yrh&zrOq_p~kCE4eq4PnTtuN&+pCNAH!;^JWd;;oiW#fjr!sxK~Ra z?rq#_!Na|gdjem>9`3E&Q<1L#5BCn-E5O6OoqJ8GfqMt;snSGC4cvQhkAsJM7w+Lw z1NT1Mqq7>g_u?L_@Nn-^b=>pU2{u~>?oYyfL(kW7`5wo0`nYTmz|QAQ3$);C z^Y>@?Tsug7*F7Tji!XI%mt(v$={J2^?dA{nIO4}!!2L0sHp}U}6y}R!SKm%OpF`YN z+s0Cx=mO;+j*+%*I=S zj}IWpIbPhJYq3dWXA)fF>(?hr5P=CM$Y> z1Mc~$t;TK+G-rv-Pd_MYNbhj(CM&obC|^PE+@CQ9$!`wn9qxXxhG&R7_1&Q9H;1^R zcRyIe6YdT`17@#qe+%w>kHZ)yuQ{amuk?=58@Rt;-qrc(2W5r3A&^(#&V0rgB)>T} zoC&-?bZ^htuJ+14C~FSu-bM88MrApMh7;}f=-2pBs*C~GV%^GIWq+~&;P?aX!6kIL{w?_B{I!ER>seq*uQGKG09 z7ncg0|K#^LmzVF>((}i(*dsUjLPcr!Fd8~d0y*~OsQUhS0;na?ld$)Ya+>?TdFyfR z>Cp1M*K_stPr0e=(n&|dqbwP)P( zFjg;a&87@)-|mhV;aX9v89d$aVb-TxS;mJ^H-;2x(>JZd#eLe|OilFq*VU;wW+lrf z&a7P9JfEz0xNr3^p$ENBfV`6e?mJK>bisWB(?hr5P=CM$XualbsyBGH#En&Vby zP48F5i<*EY|N0Rjn@5fe@A%Z)L+D+k_qgDk50c1krR$|#cC4(Lqt|8Sex!yy+-2NX z=e}kyJ})bJ*Ni#b;jST|$%@|Lt|6ewir(R_A)v{M-r=qxpvj8f;jST|$%@|Lt|6ew zir(R_A)v{M-r=qxpvj8f;jST|$*R?R{;HbhxYb$Vt{DL__b0y`rr|LwE8J(1Ugv@P zn#Zg)xX&WJ&I9)~k6CMQpGA6|M>~N|qt5(&Js?xo;64TOCQ1J3-%aO7bWq~uAI)7@;JF_;P2#IWAFcNliMcCqpUZp62KR-;+*i2I<-KHs`$A&w>qm2!&Lg=G zJXh)0Y#*(61@P~jaM!>;9{2UbmhY(+R_+7e!)aPSWhtenDqsHFTsbeM?`JM=j7@oC z8y`^b#gnYJa|XD_^x^l;clvJ2{dPA?Qrx|9;`$-=?gFajE**hQ*5pD9)!Hl1z{w{J6~?5@3R%QACoxX+}0%k?gG#&=fUj_cK10q(*x{eX4G z`F{?n>-z`mU3luEcMbdla(5OGr`w=mf%_n^Mp*CF-w5+U=69LDAMU#9EFey|L5iZz z3<7I}g}VW0kokbz-{gH0_r(TD`f1n$&S%j3(-9wK^eTz@Oy3cJdtaX3XmIzE>g&h* z)xJEv(a^h>RA0Y$(L3|^zT@tT<~=KY(O$vJdqn9Qd&1p4;C%w;#N6pNXl&p<2&@tI zfZVxHV2t?~GxM1-T&<{Zw+M}~ySX!;={f*z?$QsIy|dk}Xod!R7w*#2ZNLb?eE?KP z$amo`EprZzQ5U#JiS_Y^dmoO`DDT4kaMxAt6GWxj#UH(Q!59t#_b{?98+hLds_(xu zkL)ts!606B1j7p5sWTe*@523Xb%uGQMDUOGem<8w#C5f#(cXTX&U0i#1R&y4TC3ASJ_D^23AT=HHXY$=Z@59Ez{;(j~U zo7=6mXU!*2|3$;SJd$fW{k{H6FS#e}SIH~e@^bCJnQ|*{aqpgeQ(qSTwYmJgZMyXE V+MPh=eisz(cR+@$ZYke-{s$d!YtsM# literal 112127 zcmeI5ZIb-Z35PUZ9JU~_1R91`g)uim+`4NPXgBmU>C8erJd8NB3uZ}p1GiNI* zc&$ z?Vvv|MO(h9B=q=IxPgk=bl9p>@t_&T;xK0R2saKV>d>ib2$GURggK$ajtm=@^9>~# z>XrYXRNc;vDBaVwp^)~&IL^ldvLMnRcHvk0M-6TH&K=9iq=9dlo|m$(!7#k}in>R! z=Yx{&9gR@~m2gMn2rv~Kp~C&5X}Se_(n>Ci#eCi`9KMaO#PUMTsPAGu*~U`D1KB=p zW6cJRPWP+$kvp0g2vURm?tkSD%Q$lFZaCI4>2@u@n1g5nv7Ow|A4S$bAi(kX$w#q( zKx{_)#T=jklt)l73eg{R`~v_ek_3hXg8ue53`xI~@(kTRj>Eo>_Ol{6vv4q+{Cq?h zn2NpMX~rX~vl1el^wg?GY$-o;1|ph%lRFfVr4Q{UOf zHCT2G-4-0i36WXXaA-6AutSGIq>ir+Lqfx{pl*zs)1c!%4hDuBCY`Sahx1pLS4t>E z0$fTkgfktZ<^X~fwn-#?5o6_CmI2`+DX^{Zn zqKD$yNacXMR2uc+_pRgN0U5*V?DoLm0>anu-tfRx3ZB#?Nve09{-V6(RgaQyU`XUk zMC~e4h?&yeW4zY z0gZ;xkE5ex<$$-~lWlfh($1Va5(tWb0NB7_dEjcc=(9QW&rA=&WAJ;LYg-t`KD9Vf zRt`xlaPfi=ZJk3l(`&sl1X&!y3K0NoVi=Z}-0*A}^O!+@skuy}CO0AXQ%-}_~ZnXp?-QP(o$HMccXzZ4CO#}(BRg*1Ur(=z%ccDNExP&=*WkIi-kiB7vv@C z@=QF$AJT9}LP%O7|8FT^v&H0Kj3e$3x6XjATG!NKLbpZ7FrDXU4$kNTyf=i9tyq zVxW@E8MIarG03DXk#_5Crf+wJOcZia1BQ-x;1uL6Q5e9%kYuT~-aeiK2pBP(&&4SQ z(7?!erOmuPXcC{!K``Kzx0Yx){}{J}p*Js`pBtD2ecOn<#4|F%;DP9<&yG$`z8r%@ zf&e;wSk@Ihh}fa#|2bdNSsv{O)>`NmZ2&D*HK* z5C{m-najE4)nmhgy5~ZS4=UN5!C#hCrp@NC=V!|~kAZ0vt3y#CzK?bqMi}EE|Le-5 zO={83x3Ntje^s()2&{D-1F7y}C7F99*ob&g!C;RE2A5j(!Ow9L3qVlCun`R8{D8@) zFkoufCKWwwBoM}!luS@t(PnzBZ->BRP_Aub7{I|}@O$&~!&hPGYT@UK|;vs>+Q(vjR6AS<}C>h6SFn}NJzev%@fsFjBc;IkQLLUVKHSS#=JO+j&ERbdlc;zuJAz`?-J~cXOyVq$7 z1WZhYq2-{{X{Wq+E(hlI0pXLH+G$b;1?!_vF%42j4L;F!WV z4upVF;K|U;Av3i1-a$N-67KPVSu_F}9H4(>Ku$Fh5ck{XZPhOt4I2!= zEa0N`ehC(*@zDKD12ax;9UK?}M1ZiOhm=L=gV$k`FHkVwx8@cOGwB424h}pS;fn*2 zyS0CO)@g|Y_SA=gTO&?zKrF@WtVt(iFD~iefEW-bXeU3>r^C`1rxPmVn#~zXNjfi$ zF{zNaslsM@(j_Rk>7*vrApqB4gwvXWokN&NV$%qd5`|2~(;NUn0Px<-VvN(81kG?m zx8%C>VBpj+iJvq+$$|ZkUIZqs(O?oRF*hx028d0YGq_b!)5pLtwPQ%xW_lcwL|`Y_ z5#>!Iie9G+3S^tj8E{bdt-U5ib?haZ>7jp*0j`_~1_;pk01-?<0Y=bfa|WNssTnlX zBsQr1#b&zi*J}?qlq=xUwskozvM1I^(dLYNF3M5iOddHkym5_to9V7oOeFkZKH<-q zt&2n9oPe=8lP}X_L$ok)zrtp^^yRc?{gD$+Z38)k=&(6cd{1DYN&BK#&e}}p4qOmO zK%8QLi%vQnG_Uw#`+S-VKqx19>7+?DOU!jAI7lv%*V7oza>yKvSjZCdI-RA|595G% zpow%kBMG~qPM^77LOkfZjG-oF6Xxj991k)Bg~a)2IGvX&{)k05K{=-VmIoa4;`T`# z;4}wBXE+!Ht4JTKA_Ow)42REMAAlj7Nhbnh?Brt{SaWUhpzR)OQk*L-C39Xvr{uNv z;KWg<(`g)_Ocu!Vbc@s6hCI&}h)$E5N3JzAatQ99P+nis<8-isge0e!0rtjtAP)K& zqCJ>uQg+oDIl$WiNr{vj;Im@jD7SaZ@c@Ql>zNu|5a?nX2Qc9I7i>RWXcw&NqTF%v zsbB&-Q*B+V{A3Ok#*87k$p|VK3mub>8HluQP(I*tJ6__z3q-WL!=xH$Q=Kgy&~>!C zt8~_B8dMyV(Yc%$V&kAmoHa@8>3g+t7_>G(w=+(A1Tq(L1Wd!iffxAX4W=_D^{F4P zws9DmQj#?n<@dB5k5^oxyy4(<=)pbgg&*pAu-gX%sP?GTXR+q)&zKXrYuDZSsi zeeyX5jAOq~I&0#P&8l{jcsdMyY0pdg9!Vb^%2PJ5I$icqt+8;B)h>EsHEAS>b`Dt( zmnyJB;#KsmZ02eVPH*;6Ibq^}+odEX3?_|4zOSU9+F z2;Fa+lp(y}uq__wZvet`aveWE^ud=Dphmjsqx4MLkXvmF#(&+AKU_lxc=L`nvB0a3 zD&_z)l0Q`2tP#19C8noyfCl5YO~Lwx+Uzzz;W>n~M6B~R_Z{=RzHd0=NF)pepJ{6z z0dWQfi$k+ymo&ueN5+fWiib;tKIoL<;nVJ04?OzJ)0FM-MVn+>@i2oV$3sE^_hd8d zkTIXVPG70Z%1c+O+uk>3KZQe@npQlt%QvhgaY*i2k)Y=0C9GF(+=N4V>pD{ienxsD z2KRo+Q#ia50XvO&pr2%$84qvf@TBLsH*?U@Fn67PGlvxqPoU5DiOK4);$aRIdY;CX zJjbnddIkx;UrIlInED(yLxE?E2@dMEb#a&i23bgj!{V?wOi|z&gZj@O;zGUF=^13e zAu`}wr+2>LIYazp%x8TLlo5%=Lfm?vN+7)fG^IZ+u|^X1ASL8 z#ju_qW?-Ptx^KNn>%C#hb$Z3ao?cg-Bh~R&>ymA8xJv82emNf0gWTeNW*pN@#NPG6_?cF&~x%1hhw z+;#@xpl(~SK+N54^ZiK!RXE`7SG?WfHiCkR@TM$EILj^gg)5|r+xiPpj|%*_Jzqvm zQ9+yUHnyV5C&_c0ElvzCdVSBr@#l6U6hiu8&}NJwcV`%#Y0R+|Uuy60nJeb9+%#Y3*q9mk|PKLy7%VsW@b_k4_XzmzL7=eS5q zc!Owdw<|#Lpl_EJhuaxoK*pd*to30A2vS{9z?;uA?0EZ_6c75gb>*cgQ=+mwtaz9} zf;eP(Y27bPz(DIXC8GEchti6N2_UdMWDCUNFoi?DPOtl=2^jGFkf)^;4--Jp?9Vu? z^CxjwACGyy~9erf9S!;AA$zD_R=lj0$+3{qO_!vqk|S|8})W$N-Y0e~B_Bp=Oi$fX>D=$q+%TIcq z#&mdZUedt;DW*&a8gy|`EEb0e6S2%ov)Y*mx?2K`5)zLZ(hX`+tDwIxv|QKF zwXw#;kPBa zn;NxrL~Z=&i-T8!IE)l{acIgfr|(+_v+}dxrSU-ju7g(~?3?%v#<23zNe+$arn#A) z1uu<aPeKf#UUgH_xvE!G@0;K5tH+#qVEg{@DoR0>vYfp z>w_0csavg7Ep`ufE)Qd`bvluvPGMK$vnL&!FnD7+Jv-d}*EKp+xo_QGr~AA}!oe45 z&&vblC7+sJk(a#xQP2tpyw6uQOls=I*k@Y_z0mI)Inb=jw>pQiU0IX&baY(p0TgiJ zUGdPyA;)cFpFB zztAf~;JoCmE2fV&_w$YlhrFlSm5Vr}LhAupebHY+gUa4C?8)xzg3mtOTJkhr#Uai)rOmY*G;`!5&P50T`XfJ7O15J9C~H6PAa^MGs+_r&gBnT>90~^E zctq1)@cBb@R*7>dhk7`0C`1m$^6=g3!;Kug{C*$6yIAmH5LiUpzJ8rfTn)0IupbYF zlIq%{gW~D{z!TuTS|7gsektLg0gkQ*tOZEQyD-`XDc%>5;OkyKo?SheIhS`MCwX;3wHKyR#lX?=YKKiKoh! z!+6-nuzg025Dq*Ay3Q9Fa$?%o-?yf|24PUx;^0O0`}88ItDOn0AauT=#siQ&9~R-D z;yCZ*FvkKKx%cVG(RNuHhZtB!JxKshg7DosWV$~|$p+LrghAeCDuw#v3QQ-f4 C;eA*D diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml index ff3eddf..899938d 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml @@ -1,7 +1,7 @@ image: interbotix_map.pgm mode: trinary resolution: 0.05 -origin: [-9.38, -5.22, 0] +origin: [-9.38, -5.11, 0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.25 From f25de9cd5371227f959ca63b15afccdbfc885380 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Sat, 11 Mar 2023 11:02:17 +0000 Subject: [PATCH 06/13] [xslocobot_nav] Fix naming conflict --- .../launch/xslocobot_slam_toolbox.launch.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py index 0c1b5c1..ed43cd9 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py @@ -62,7 +62,7 @@ def launch_setup(context, *args, **kwargs): xs_driver_logging_level_launch_arg = LaunchConfiguration('xs_driver_logging_level') use_rviz_launch_arg = LaunchConfiguration('use_rviz') slam_toolbox_params_file_launch_arg = LaunchConfiguration('slam_toolbox_params_file') - slam_mode_launch_arg = LaunchConfiguration('slam_mode') + slam_toolbox_mode_launch_arg = LaunchConfiguration('slam_toolbox_mode') camera_tilt_angle_launch_arg = LaunchConfiguration('camera_tilt_angle') launch_driver_launch_arg = LaunchConfiguration('launch_driver') hardware_type_launch_arg = LaunchConfiguration('hardware_type') @@ -114,7 +114,7 @@ def launch_setup(context, *args, **kwargs): shell=True, ) slam_toolbox_online_sync_slam_node = Node( - condition=LaunchConfigurationEquals('slam_mode', 'online_sync'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'online_sync'), package='slam_toolbox', executable='sync_slam_toolbox_node', name='slam_toolbox', @@ -128,7 +128,7 @@ def launch_setup(context, *args, **kwargs): ) slam_toolbox_online_async_slam_node = Node( - condition=LaunchConfigurationEquals('slam_mode', 'online_async'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'online_async'), package='slam_toolbox', executable='async_slam_toolbox_node', name='slam_toolbox', @@ -142,7 +142,7 @@ def launch_setup(context, *args, **kwargs): ) slam_toolbox_localization_slam_node = Node( - condition=LaunchConfigurationEquals('slam_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), package='slam_toolbox', executable='localization_slam_toolbox_node', name='slam_toolbox', @@ -170,7 +170,7 @@ def launch_setup(context, *args, **kwargs): 'autostart': 'true', 'params_file': LaunchConfiguration('nav2_params_file'), 'use_slam_toolbox': 'true', - 'slam_mode': slam_mode_launch_arg, + 'slam_toolbox_mode': slam_toolbox_mode_launch_arg, 'map': map_yaml_file_launch_arg, }.items(), ) @@ -274,7 +274,7 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - 'slam_mode', + 'slam_toolbox_mode', default_value='online_async', choices=( # 'lifelong', @@ -295,7 +295,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( 'slam_toolbox_params_filename', - default_value=('slam_toolbox_', LaunchConfiguration('slam_mode'), '.yaml') + default_value=('slam_toolbox_', LaunchConfiguration('slam_toolbox_mode'), '.yaml') ) ) declared_arguments.append( From 344c9e1c45232ec8f6bb3fdf7fee8d8e2d76a559 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Mon, 22 May 2023 13:49:49 +0200 Subject: [PATCH 07/13] [xslocobot_nav] Use composition node --- .../launch/xslocobot_nav2_bringup.launch.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index b7a4526..a94303f 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -61,6 +61,7 @@ def launch_setup(context, *args, **kwargs): use_sim_time_launch_arg = LaunchConfiguration('use_sim_time') log_level_launch_arg = LaunchConfiguration('log_level') autostart_launch_arg = LaunchConfiguration('autostart') + use_composition_launch_arg = LaunchConfiguration('use_composition') use_respawn_launch_arg = LaunchConfiguration('use_respawn') nav2_params_file_launch_arg = LaunchConfiguration('nav2_params_file') cmd_vel_topic_launch_arg = LaunchConfiguration('cmd_vel_topic') @@ -183,6 +184,15 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(use_slam_toolbox_launch_arg), actions=[ SetParameter('use_sim_time', use_sim_time_launch_arg), + Node( + condition=IfCondition(use_composition_launch_arg), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart_launch_arg}], + arguments=['--ros-args', '--log-level', log_level_launch_arg], + remappings=tf_remappings, + output='screen'), Node( condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization_amcl'), package='nav2_map_server', @@ -283,6 +293,14 @@ def generate_launch_description(): description='automatically startup the Nav2 stack.', ) ) + declared_arguments.append( + DeclareLaunchArgument( + 'use_composition', + default_value='true', + choices=('true', 'false'), + description='Whether to use composed bringup', + ) + ) declared_arguments.append( DeclareLaunchArgument( 'cmd_vel_topic', From b205a5ada5cdd77c67fd940503764c6b78b56587 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Mon, 22 May 2023 13:51:13 +0200 Subject: [PATCH 08/13] [xslocobot_nav] Camera launch argument --- .../launch/xslocobot_slam_toolbox.launch.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py index ed43cd9..4218310 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py @@ -63,6 +63,7 @@ def launch_setup(context, *args, **kwargs): use_rviz_launch_arg = LaunchConfiguration('use_rviz') slam_toolbox_params_file_launch_arg = LaunchConfiguration('slam_toolbox_params_file') slam_toolbox_mode_launch_arg = LaunchConfiguration('slam_toolbox_mode') + use_camera_launch_arg = LaunchConfiguration('use_camera') camera_tilt_angle_launch_arg = LaunchConfiguration('camera_tilt_angle') launch_driver_launch_arg = LaunchConfiguration('launch_driver') hardware_type_launch_arg = LaunchConfiguration('hardware_type') @@ -93,7 +94,7 @@ def launch_setup(context, *args, **kwargs): 'use_rviz': use_rviz_launch_arg, 'use_base_odom_tf': use_base_odom_tf_launch_arg, 'rviz_frame': 'map', - 'use_camera': 'true', + 'use_camera': use_camera_launch_arg, 'rs_camera_align_depth': 'true', 'use_base': 'true', # 'use_dock': 'true', @@ -224,6 +225,14 @@ def generate_launch_description(): description='set the logging level of the X-Series Driver.' ) ) + declared_arguments.append( + DeclareLaunchArgument( + 'use_camera', + default_value='false', + choices=('true', 'false'), + description='if `true`, the RealSense camera nodes are launched.', + ) + ) declared_arguments.append( DeclareLaunchArgument( 'camera_tilt_angle', From 03906ee681683e34e891f76ac8b167f1b468374b Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Tue, 23 May 2023 17:37:54 +0000 Subject: [PATCH 09/13] [xslocobot_sim] Adjust sim RViZ --- .../rviz/xslocobot_gz_classic.rviz | 242 +++++++++++++----- 1 file changed, 171 insertions(+), 71 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz b/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz index c57e597..01d1c44 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_sim/rviz/xslocobot_gz_classic.rviz @@ -472,49 +472,172 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false - - Alpha: 1 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true + - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: Map + Name: Landmark Nav Goals + Namespaces: + {} Topic: Depth: 5 - Durability Policy: Transient Local + Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /map - Use Timestamp: false - Value: true - - Alpha: 5 - Class: nav2_rviz_plugins/ParticleCloud - Color: 0; 180; 0 + Value: /marker_viz + Value: false + - Class: rviz_common/Group + Displays: + - Class: rtabmap_rviz_plugins/Info + Enabled: true + Name: Info + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/info + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rtabmap_rviz_plugins/MapCloud + Cloud decimation: 4 + Cloud from scan: false + Cloud max depth (m): 4 + Cloud min depth (m): 0 + Cloud voxel size (m): 0.009999999776482582 + Color: 255; 255; 255 + Color Transformer: RGB8 + Download graph: false + Download map: false + Download namespace: rtabmap + Enabled: false + Filter ceiling (m): 0 + Filter floor (m): 0 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: MapCloud + Node filtering angle (degrees): 30 + Node filtering radius (m): 0 + Position Transformer: XYZ + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/mapData + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Class: rtabmap_rviz_plugins/MapGraph + Enabled: true + Global loop closure: 255; 0; 0 + Landmark: 0; 128; 0 + Local loop closure: 255; 255; 0 + Merged neighbor: 255; 170; 0 + Name: MapGraph + Neighbor: 0; 0; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/mapGraph + User: 255; 0; 0 + Value: true + Virtual: 255; 0; 255 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ObstacleCloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /locobot/rtabmap/cloud_obstacles + Use Fixed Frame: true + Use rainbow: true + Value: false Enabled: false - Max Arrow Length: 0.3 - Min Arrow Length: 0.02 - Name: Amcl Particle Swarm - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /particle_cloud - Value: true + Name: RTAB-Map - Class: rviz_common/Group Displays: - - Alpha: 0.5 + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: costmap - Draw Behind: false + Draw Behind: true Enabled: true Name: Global Costmap Topic: Depth: 5 - Durability Policy: Transient Local + Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates Use Timestamp: false Value: true - Alpha: 1 @@ -545,23 +668,6 @@ Visualization Manager: Reliability Policy: Reliable Value: /plan Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/published_footprint - Value: false - Enabled: true - Name: Global Planner - - Class: rviz_common/Group - Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: costmap @@ -570,7 +676,7 @@ Visualization Manager: Name: Local Costmap Topic: Depth: 5 - Durability Policy: Transient Local + Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable @@ -587,7 +693,7 @@ Visualization Manager: Buffer Length: 1 Class: rviz_default_plugins/Path Color: 0; 12; 255 - Enabled: true + Enabled: false Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 @@ -610,24 +716,12 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /local_plan - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Trajectories - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /marker Value: false - Alpha: 1 Class: rviz_default_plugins/Polygon Color: 25; 255; 0 Enabled: true - Name: Polygon + Name: Robot Footprint Topic: Depth: 5 Durability Policy: Volatile @@ -636,11 +730,11 @@ Visualization Manager: Reliability Policy: Reliable Value: /local_costmap/published_footprint Value: true - Enabled: true - Name: Local Planner + Enabled: false + Name: Navigation Enabled: true Global Options: - Background Color: 200; 200; 200 + Background Color: 0; 176; 240 Fixed Frame: locobot/odom Frame Rate: 30 Name: root @@ -684,7 +778,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.381117105484009 + Distance: 2.5063586235046387 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -698,21 +792,27 @@ Visualization Manager: Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5553984045982361 + Near Clip Distance: 0.01 + Pitch: 0.7853981852531433 Target Frame: - Value: Orbit (rviz) - Yaw: 0.8953980803489685 + Value: Orbit (rviz_default_plugins) + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: + Camera Image: + collapsed: false Displays: collapsed: false - Height: 923 + Height: 752 Hide Left Dock: false - Hide Right Dock: false - Image: + Hide Right Dock: true + Interbotix Control Panel: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000018b00000368fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000001600000221000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000002420000013c0000000000000000fb0000000a0049006d006100670065010000023d000001410000002800fffffffb0000000c00430061006d0065007200610100000289000000f50000000000000000fb0000000a0049006d00610067006501000002b6000000c800000000000000000000000100000110000002a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002a20000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004770000036800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Width: 1544 + QMainWindow State: 000000ff00000000fd00000003000000000000016d00000296fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000000000000000fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000296000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000300049006e0074006500720062006f00740069007800200043006f006e00740072006f006c002000500061006e0065006c00000001910000012e0000012e00000151fb0000000a0049006d00610067006501000002ac000000d20000000000000000fb0000000a0049006d006100670065010000025f0000011f0000000000000000fb0000001800430061006d00650072006100200049006d00610067006500000002b10000012a0000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000347000000af0000005c00ffffff00000001000001000000039efc0200000004fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000000a00560069006500770073000000003d0000039e0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000003000006080000004cfc0100000002fb0000000800540069006d00650100000000000006080000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003f70000029600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1386 X: 72 Y: 27 From 3ad738763807cc4fe784cb07242b2edb0b6f0d31 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Tue, 23 May 2023 18:15:30 +0000 Subject: [PATCH 10/13] [xslocobot_nav] Fix nav2_bringup --- .../launch/xslocobot_nav2_bringup.launch.py | 31 ++++++------------- .../launch/xslocobot_slam_toolbox.launch.py | 8 ++--- 2 files changed, 12 insertions(+), 27 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index a94303f..8fc18c9 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -41,7 +41,6 @@ from launch.conditions import ( IfCondition, LaunchConfigurationEquals, - LaunchConfigurationNotEquals, ) from launch.substitutions import ( PathJoinSubstitution, @@ -194,7 +193,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings, output='screen'), Node( - condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_mode', 'mapping'), package='nav2_map_server', executable='map_saver_server', name='map_saver_server', @@ -207,7 +206,7 @@ def launch_setup(context, *args, **kwargs): parameters=[configured_params], ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), package='nav2_map_server', executable='map_server', name='map_server', @@ -221,7 +220,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), package='nav2_amcl', executable='amcl', name='amcl', @@ -233,7 +232,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationNotEquals('slam_toolbox_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'mapping'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', @@ -248,7 +247,7 @@ def launch_setup(context, *args, **kwargs): ] ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization_amcl'), + condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', @@ -345,22 +344,10 @@ def generate_launch_description(): ) declared_arguments.append( DeclareLaunchArgument( - 'slam_toolbox_mode', - default_value='online_async', - choices=( - # 'lifelong', - 'localization', - 'localization_amcl', - # 'offline', - 'online_async', - 'online_sync' - ), - description=( - "the mode to launch the SLAM in using the slam_toolbox. Currently only " - "'localization', 'online_sync', and 'online_async' modes are supported." - "'localization' mode takes a pose graph map defined in the slam_toolbox_localization.yaml" - "'localization_amcl' mode instead uses the amcl package to localize, given a map yaml file." - ), + 'slam_mode', + default_value='mapping', + choices=('mapping', 'localization'), + description='the mode to launch the SLAM in using RTAB-MAP or SLAM Toolbox.', ) ) declared_arguments.append( diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py index 4218310..caf4420 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_slam_toolbox.launch.py @@ -171,7 +171,7 @@ def launch_setup(context, *args, **kwargs): 'autostart': 'true', 'params_file': LaunchConfiguration('nav2_params_file'), 'use_slam_toolbox': 'true', - 'slam_toolbox_mode': slam_toolbox_mode_launch_arg, + 'slam_mode': 'mapping', 'map': map_yaml_file_launch_arg, }.items(), ) @@ -287,17 +287,15 @@ def generate_launch_description(): default_value='online_async', choices=( # 'lifelong', - 'localization', - 'localization_amcl', + # 'localization', # 'offline', 'online_async', 'online_sync' ), description=( "the mode to launch the SLAM in using the slam_toolbox. Currently only " - "'localization', 'online_sync', and 'online_async' modes are supported." + "'online_sync', and 'online_async' modes are supported." "'localization' mode takes a pose graph map defined in the slam_toolbox_localization.yaml" - "'localization_amcl' mode instead uses the amcl package to localize, given a map yaml file." ), ) ) From 9ece370111f2d2edaa57d51d0d404044c83670aa Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Tue, 23 May 2023 18:18:51 +0000 Subject: [PATCH 11/13] [xslocobot_nav] Fix argument naming --- .../launch/xslocobot_nav2_bringup.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index 8fc18c9..e8edade 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -206,7 +206,7 @@ def launch_setup(context, *args, **kwargs): parameters=[configured_params], ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization'), package='nav2_map_server', executable='map_server', name='map_server', @@ -220,7 +220,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization'), package='nav2_amcl', executable='amcl', name='amcl', @@ -232,7 +232,7 @@ def launch_setup(context, *args, **kwargs): remappings=tf_remappings ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'mapping'), + condition=LaunchConfigurationEquals('slam_mode', 'mapping'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_slam', @@ -247,7 +247,7 @@ def launch_setup(context, *args, **kwargs): ] ), Node( - condition=LaunchConfigurationEquals('slam_toolbox_mode', 'localization'), + condition=LaunchConfigurationEquals('slam_mode', 'localization'), package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', From 91e06bfcd72e3ca9252015a2f64578fced3cc31c Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Tue, 13 Jun 2023 13:16:51 +0200 Subject: [PATCH 12/13] [xslocobot_nav] Fix map saver server Fix bug Waiting for service map_server/get_state --- .../launch/xslocobot_nav2_bringup.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index e8edade..133e1bb 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -191,12 +191,12 @@ def launch_setup(context, *args, **kwargs): parameters=[configured_params, {'autostart': autostart_launch_arg}], arguments=['--ros-args', '--log-level', log_level_launch_arg], remappings=tf_remappings, - output='screen'), + output='screen' + ), Node( condition=LaunchConfigurationEquals('slam_mode', 'mapping'), package='nav2_map_server', executable='map_saver_server', - name='map_saver_server', output='screen', respawn=use_respawn_launch_arg, respawn_delay=2.0, @@ -204,6 +204,7 @@ def launch_setup(context, *args, **kwargs): '--ros-args', '--log-level', log_level_launch_arg ], parameters=[configured_params], + remappings=tf_remappings ), Node( condition=LaunchConfigurationEquals('slam_mode', 'localization'), From 107c43dc490b4e8fa247c2ac986a058ca7139856 Mon Sep 17 00:00:00 2001 From: Stephen Adhisaputra Date: Tue, 20 Jun 2023 19:31:29 +0000 Subject: [PATCH 13/13] [xslocobot_nav] Adjust nav2 launch Adjust launch file to accomodate both rtabmap and slam_toolbox --- .../launch/xslocobot_nav2_bringup.launch.py | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py index 133e1bb..215fdc7 100644 --- a/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py +++ b/interbotix_ros_xslocobots/interbotix_xslocobot_nav/launch/xslocobot_nav2_bringup.launch.py @@ -64,7 +64,6 @@ def launch_setup(context, *args, **kwargs): use_respawn_launch_arg = LaunchConfiguration('use_respawn') nav2_params_file_launch_arg = LaunchConfiguration('nav2_params_file') cmd_vel_topic_launch_arg = LaunchConfiguration('cmd_vel_topic') - use_slam_toolbox_launch_arg = LaunchConfiguration('use_slam_toolbox') map_yaml_file_launch_arg = LaunchConfiguration('map') # Set env var to print messages to stdout immediately set_logging_env_var = SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -180,7 +179,6 @@ def launch_setup(context, *args, **kwargs): ) slam_toolbox_nav2_nodes = GroupAction( - condition=IfCondition(use_slam_toolbox_launch_arg), actions=[ SetParameter('use_sim_time', use_sim_time_launch_arg), Node( @@ -333,16 +331,6 @@ def generate_launch_description(): ) ) ) - declared_arguments.append( - DeclareLaunchArgument( - 'use_slam_toolbox', - default_value='false', - choices=('true', 'false'), - description=( - 'whether to use slam_toolbox over rtabmap.' - ) - ) - ) declared_arguments.append( DeclareLaunchArgument( 'slam_mode', @@ -374,4 +362,4 @@ def generate_launch_description(): ) - return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) \ No newline at end of file