From e14706562a2a2f99e7cc9fb6533ff6a589e5f099 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 11:33:33 +0900 Subject: [PATCH 1/6] [navigation_stage] add tf_prefix argument to amcl_node.xml and move_base.xml --- navigation_stage/move_base_config/amcl_node.xml | 5 ++++- navigation_stage/move_base_config/move_base.xml | 13 +++++++------ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/navigation_stage/move_base_config/amcl_node.xml b/navigation_stage/move_base_config/amcl_node.xml index 2b61f74..7ac050d 100644 --- a/navigation_stage/move_base_config/amcl_node.xml +++ b/navigation_stage/move_base_config/amcl_node.xml @@ -1,4 +1,5 @@ + @@ -31,7 +32,9 @@ - + + + diff --git a/navigation_stage/move_base_config/move_base.xml b/navigation_stage/move_base_config/move_base.xml index 4d15386..d8a809e 100644 --- a/navigation_stage/move_base_config/move_base.xml +++ b/navigation_stage/move_base_config/move_base.xml @@ -1,4 +1,5 @@ + @@ -14,13 +15,13 @@ --> - - - - - + + + + + From a9c48be68c533d2a5a0eb50629153e7a593ffcab Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 16:43:36 +0900 Subject: [PATCH 2/6] [navigation_stage] add map to /map remap to move_base in move_bae.xml --- navigation_stage/move_base_config/move_base.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/navigation_stage/move_base_config/move_base.xml b/navigation_stage/move_base_config/move_base.xml index d8a809e..31dc5f5 100644 --- a/navigation_stage/move_base_config/move_base.xml +++ b/navigation_stage/move_base_config/move_base.xml @@ -4,6 +4,7 @@ Example move_base configuration. Descriptions of parameters, as well as a full list of all amcl parameters, can be found at http://www.ros.org/wiki/move_base. --> + From 43eee57e7ba671773aab136e22570fa086c5a18b Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 16:44:28 +0900 Subject: [PATCH 3/6] [navigation_stage] add map topic and service remap, and add initial_pose argument --- navigation_stage/move_base_config/amcl_node.xml | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/navigation_stage/move_base_config/amcl_node.xml b/navigation_stage/move_base_config/amcl_node.xml index 7ac050d..2b7c208 100644 --- a/navigation_stage/move_base_config/amcl_node.xml +++ b/navigation_stage/move_base_config/amcl_node.xml @@ -1,10 +1,16 @@ + + + + + + @@ -39,9 +45,9 @@ - - - + + + From 417892edf691313d7d5ca5b3ae9a177292f5d1c6 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 16:45:21 +0900 Subject: [PATCH 4/6] [navigation_stage] add tf_prefix to local and global costmap_params.yaml --- navigation_stage/move_base_config/global_costmap_params.yaml | 2 +- navigation_stage/move_base_config/local_costmap_params.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/navigation_stage/move_base_config/global_costmap_params.yaml b/navigation_stage/move_base_config/global_costmap_params.yaml index 977dab7..7911c32 100644 --- a/navigation_stage/move_base_config/global_costmap_params.yaml +++ b/navigation_stage/move_base_config/global_costmap_params.yaml @@ -3,7 +3,7 @@ global_costmap: #Set the global and robot frames for the costmap global_frame: map - robot_base_frame: base_link + robot_base_frame: $(arg tf_prefix)base_link #Set the update and publish frequency of the costmap update_frequency: 5.0 diff --git a/navigation_stage/move_base_config/local_costmap_params.yaml b/navigation_stage/move_base_config/local_costmap_params.yaml index 4d2b956..a8eadf3 100644 --- a/navigation_stage/move_base_config/local_costmap_params.yaml +++ b/navigation_stage/move_base_config/local_costmap_params.yaml @@ -5,8 +5,8 @@ local_costmap: publish_voxel_map: true #Set the global and robot frames for the costmap - global_frame: odom - robot_base_frame: base_link + global_frame: $(arg tf_prefix)odom + robot_base_frame: $(arg tf_prefix)base_link #Set the update and publish frequency of the costmap update_frequency: 5.0 From b206ee4c2ba936f4a50e5f2ff56c8b8941558d26 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 16:47:18 +0900 Subject: [PATCH 5/6] [navigation_stage] changed multi_robot.launch to use xml instead of node tag, to use amcl instead of fake_localization and to launch GUI of stage_ros --- .../launch/move_base_multi_robot.launch | 56 ++++++++----------- 1 file changed, 22 insertions(+), 34 deletions(-) diff --git a/navigation_stage/launch/move_base_multi_robot.launch b/navigation_stage/launch/move_base_multi_robot.launch index fe234b5..1a0dd97 100644 --- a/navigation_stage/launch/move_base_multi_robot.launch +++ b/navigation_stage/launch/move_base_multi_robot.launch @@ -3,52 +3,40 @@ - + - + - - - - - - - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + From 8182c46099a0d310fb9ad35aa5f06840861addab Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 16:47:41 +0900 Subject: [PATCH 6/6] [navigation_stage] update rviz config for multi_robot --- navigation_stage/multi_robot.rviz | 323 ++++++++++++++++++++++++++---- 1 file changed, 280 insertions(+), 43 deletions(-) diff --git a/navigation_stage/multi_robot.rviz b/navigation_stage/multi_robot.rviz index 60d4ed7..2ef0686 100644 --- a/navigation_stage/multi_robot.rviz +++ b/navigation_stage/multi_robot.rviz @@ -6,7 +6,7 @@ Panels: Expanded: - /Global Options1 Splitter Ratio: 0.5 - Tree Height: 510 + Tree Height: 794 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -15,7 +15,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -25,125 +25,351 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: "Robot 0: LaserScan" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - - Alpha: 0.7 + - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map Draw Behind: false Enabled: true Name: Map Topic: /map + Unreliable: false + Use Timestamp: false 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: rviz/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: "Robot 0: LaserScan" + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /robot_0/base_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.5 + Arrow Length: 0.10000000149011612 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 0; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: "Robot 0: Amcl Particle" + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /robot_0/particlecloud + Unreliable: false + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 149; 0 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 Name: "Robot 0: Global Plan" + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /robot_0/move_base_node/TrajectoryPlannerROS/global_plan + Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 0; 255 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 Name: "Robot 0: Local Plan" + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /robot_0/move_base_node/TrajectoryPlannerROS/local_plan + Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 Name: "Robot 0: NavFn Plan" + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /robot_0/move_base_node/NavfnROS/plan + Unreliable: false Value: true - - Alpha: 1 - Class: rviz/GridCells - Color: 0; 0; 255 + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false Enabled: true - Name: "Robot 0: Inflated Obstacles" - Topic: /robot_0/move_base_node/local_costmap/inflated_obstacles + Name: "Robot 0: Local Costmap" + Topic: /robot_0/move_base_node/local_costmap/costmap + Unreliable: false + Use Timestamp: false Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: "Robot 0: Global Costmap" + Topic: /robot_0/move_base_node/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: false - Alpha: 1 - Class: rviz/GridCells + Class: rviz/Polygon Color: 255; 0; 0 Enabled: true - Name: "Robot 0: Obstacles" - Topic: /robot_0/move_base_node/local_costmap/obstacles - Value: true - - Alpha: 1 - Class: rviz/GridCells - Color: 0; 255; 0 - Enabled: true - Name: "Robot 0: Unknown Space" - Topic: /robot_0/move_base_node/local_costmap/unknown_space + Name: "Robot 0: Robot Footprint" + Queue Size: 10 + Topic: /robot_0/move_base_node/local_costmap/footprint + Unreliable: false Value: true - Alpha: 1 - Class: rviz/Polygon - Color: 255; 0; 0 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 0; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 Enabled: true - Name: "Robot 0: Robot Footprint" - Topic: /robot_0/move_base_node/local_costmap/robot_footprint + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: "Robot 1: LaserScan" + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /robot_1/base_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true Value: true + - Alpha: 0.5 + Arrow Length: 0.10000000149011612 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 0; 0; 255 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: "Robot 1: Amcl Particle" + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /robot_1/particlecloud + Unreliable: false + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 149; 0 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 Name: "Robot 1: Global Plan" + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /robot_1/move_base_node/TrajectoryPlannerROS/global_plan + Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 0; 0; 255 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 Name: "Robot 1: Local Plan" + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /robot_1/move_base_node/TrajectoryPlannerROS/local_plan + Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 255; 0; 0 Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 Name: "Robot 1: NavFn Plan" + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 Topic: /robot_1/move_base_node/NavfnROS/plan + Unreliable: false Value: true - Alpha: 1 Class: rviz/GridCells Color: 0; 0; 255 Enabled: true Name: "Robot 1: Inflated Obstacles" + Queue Size: 10 Topic: /robot_1/move_base_node/local_costmap/inflated_obstacles + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/GridCells + Color: 0; 255; 0 + Enabled: true + Name: "Robot 1: Unknown Space" + Queue Size: 10 + Topic: /robot_1/move_base_node/local_costmap/unknown_space + Unreliable: false Value: true - Alpha: 1 Class: rviz/GridCells Color: 255; 0; 0 Enabled: true Name: "Robot 1: Obstacles" + Queue Size: 10 Topic: /robot_1/move_base_node/local_costmap/obstacles + Unreliable: false Value: true - - Alpha: 1 - Class: rviz/GridCells - Color: 0; 255; 0 + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false Enabled: true - Name: "Robot 1: Unknown Space" - Topic: /robot_1/move_base_node/local_costmap/unknown_space + Name: "Robot 1: Local Costmap" + Topic: /robot_1/move_base_node/local_costmap/costmap + Unreliable: false + Use Timestamp: false Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: "Robot 1: Global Costmap" + Topic: /robot_1/move_base_node/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: false - Alpha: 1 Class: rviz/Polygon - Color: 255; 0; 0 + Color: 0; 0; 255 Enabled: true Name: "Robot 1: Robot Footprint" - Topic: /robot_1/move_base_node/local_costmap/robot_footprint + Queue Size: 10 + Topic: /robot_1/move_base_node/local_costmap/footprint + Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: map Frame Rate: 30 Name: root @@ -155,7 +381,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -165,25 +394,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10 + Distance: 20.920917510986328 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 Focal Point: - X: 0 - Y: 0 - Z: 0 + X: 43.63037872314453 + Y: 18.595481872558594 + Z: 24.23491859436035 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.555398 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 Target Frame: - Value: Orbit (rviz) - Yaw: 2.73721 + Yaw: 3.1522068977355957 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 756 + Height: 1017 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000002090000028cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000028c000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000028cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004c0000028c000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000001e20000028c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000209000003a3fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a3000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a3000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000045c000003a300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -192,6 +429,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1276 - X: 4 - Y: 22 + Width: 1920 + X: 0 + Y: 23