forked from xtdrone/XTDrone
modify 2d motion planning fot hectorslam
This commit is contained in:
parent
dc922527fa
commit
3ceccb6d26
|
@ -2,7 +2,7 @@
|
|||
<node pkg="tf" type="static_transform_publisher" name="odom_to_map"
|
||||
args="0.0 0.0 0 0.0 0.0 0.0 /odom /map 40" />
|
||||
<!-- Arguments -->
|
||||
<arg name="motion_planning" default="/home/robin/px4/xtdrone/motion_planning"/>
|
||||
<arg name="motion_planning" default="/home/robin/XTDrone/motion_planning/2d/"/>
|
||||
<arg name="move_forward_only" default="false"/>
|
||||
<arg name="open_rviz" default="true"/>
|
||||
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
<launch>
|
||||
<!-- Arguments -->
|
||||
<arg name="motion_planning" default="/home/robin/px4/xtdrone/motion_planning"/>
|
||||
<arg name="motion_planning" default="/home/robin/Xtdrone/motion_planning/2d"/>
|
||||
<arg name="cmd_vel_topic" default="/cmd_vel" />
|
||||
<arg name="odom_topic" default="odom" />
|
||||
<arg name="move_forward_only" default="false"/>
|
||||
|
|
File diff suppressed because one or more lines are too long
|
@ -1,6 +1,6 @@
|
|||
image: /home/robin/XTDrone/motion_planning/2d/map/indoor3.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-51.224998, -51.224998, 0.000000]
|
||||
origin: [-5.145000, -5.145000, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
|
File diff suppressed because one or more lines are too long
|
@ -0,0 +1,7 @@
|
|||
image: /home/robin/XTDrone/motion_planning/2d/map/indoor4.pgm
|
||||
resolution: 0.050000
|
||||
origin: [-51.224998, -51.224998, 0.000000]
|
||||
negate: 0
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
|
|
@ -1,18 +1,18 @@
|
|||
DWAPlannerROS:
|
||||
|
||||
# Robot Configuration Parameters
|
||||
max_vel_x: 0.2
|
||||
min_vel_x: -0.2
|
||||
max_vel_x: 1.0
|
||||
min_vel_x: -1.0
|
||||
|
||||
max_vel_y: 0.2
|
||||
min_vel_y: -0.2
|
||||
max_vel_y: 1.0
|
||||
min_vel_y: -1.0
|
||||
|
||||
# The velocity when robot is moving in a straight line
|
||||
max_trans_vel: 0.2
|
||||
min_trans_vel: 0.01
|
||||
max_vel_trans: 1.0
|
||||
min_vel_trans: 0.01
|
||||
|
||||
max_rot_vel: 0.5
|
||||
min_rot_vel: 0.01
|
||||
max_vel_theta: 0.5
|
||||
min_vel_theta: 0.01
|
||||
|
||||
acc_lim_x: 1
|
||||
acc_lim_y: 1
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
global_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: iris_0/base_link
|
||||
robot_base_frame: base_link
|
||||
|
||||
update_frequency: 10.0
|
||||
publish_frequency: 10.0
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
local_costmap:
|
||||
global_frame: map
|
||||
robot_base_frame: iris_0/base_link
|
||||
robot_base_frame: base_link
|
||||
|
||||
update_frequency: 10.0
|
||||
publish_frequency: 10.0
|
||||
|
|
|
@ -12,7 +12,7 @@ Panels:
|
|||
- /Local Map1/Costmap1
|
||||
- /Local Map1/Planner1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 843
|
||||
Tree Height: 796
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
|
@ -20,7 +20,7 @@ Panels:
|
|||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679016
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
|
@ -31,6 +31,8 @@ Panels:
|
|||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: LaserScan
|
||||
Preferences:
|
||||
PromptSaveOnExit: true
|
||||
Toolbars:
|
||||
toolButtonStyle: 2
|
||||
Visualization Manager:
|
||||
|
@ -45,7 +47,7 @@ Visualization Manager:
|
|||
Value: true
|
||||
base_link_frd:
|
||||
Value: true
|
||||
iris_0/base_link:
|
||||
base_stabilized:
|
||||
Value: true
|
||||
iris_0/laser_2d:
|
||||
Value: true
|
||||
|
@ -57,21 +59,15 @@ Visualization Manager:
|
|||
Value: true
|
||||
odom_ned:
|
||||
Value: true
|
||||
scanmatcher_frame:
|
||||
Value: true
|
||||
Marker Scale: 3
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
odom:
|
||||
iris_0/base_link:
|
||||
iris_0/laser_2d:
|
||||
{}
|
||||
map:
|
||||
map_ned:
|
||||
{}
|
||||
odom_ned:
|
||||
{}
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
|
@ -97,33 +93,33 @@ Visualization Manager:
|
|||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.100000001
|
||||
Size (m): 0.10000000149011612
|
||||
Style: Flat Squares
|
||||
Topic: /iris_0/scan
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.699999988
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /map
|
||||
Unreliable: false
|
||||
Use Timestamp: false
|
||||
Value: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 0; 0; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.0299999993
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Planner Plan
|
||||
Offset:
|
||||
X: 0
|
||||
|
@ -131,15 +127,15 @@ Visualization Manager:
|
|||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /move_base/NavfnROS/plan
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Group
|
||||
Displays:
|
||||
- Alpha: 0.699999988
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: true
|
||||
|
@ -154,11 +150,11 @@ Visualization Manager:
|
|||
Class: rviz/Path
|
||||
Color: 255; 0; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.0299999993
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Planner
|
||||
Offset:
|
||||
X: 0
|
||||
|
@ -166,9 +162,9 @@ Visualization Manager:
|
|||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /move_base/DWAPlannerROS/global_plan
|
||||
Unreliable: false
|
||||
Value: true
|
||||
|
@ -184,7 +180,7 @@ Visualization Manager:
|
|||
Topic: /move_base/local_costmap/footprint
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 0.699999988
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz/Map
|
||||
Color Scheme: costmap
|
||||
Draw Behind: false
|
||||
|
@ -199,11 +195,11 @@ Visualization Manager:
|
|||
Class: rviz/Path
|
||||
Color: 255; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.0299999993
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Planner
|
||||
Offset:
|
||||
X: 0
|
||||
|
@ -211,9 +207,9 @@ Visualization Manager:
|
|||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /move_base/DWAPlannerROS/local_plan
|
||||
Unreliable: false
|
||||
Value: true
|
||||
|
@ -221,15 +217,15 @@ Visualization Manager:
|
|||
Name: Local Map
|
||||
- Alpha: 1
|
||||
Axes Length: 1
|
||||
Axes Radius: 0.100000001
|
||||
Axes Radius: 0.10000000149011612
|
||||
Class: rviz/Pose
|
||||
Color: 255; 25; 0
|
||||
Enabled: true
|
||||
Head Length: 0.300000012
|
||||
Head Radius: 0.100000001
|
||||
Head Length: 0.30000001192092896
|
||||
Head Radius: 0.10000000149011612
|
||||
Name: Goal
|
||||
Shaft Length: 0.5
|
||||
Shaft Radius: 0.0500000007
|
||||
Shaft Radius: 0.05000000074505806
|
||||
Shape: Arrow
|
||||
Topic: /move_base_simple/goal
|
||||
Unreliable: false
|
||||
|
@ -239,11 +235,11 @@ Visualization Manager:
|
|||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.300000012
|
||||
Head Length: 0.200000003
|
||||
Length: 0.300000012
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.0299999993
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
|
@ -251,13 +247,13 @@ Visualization Manager:
|
|||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.0299999993
|
||||
Shaft Diameter: 0.100000001
|
||||
Shaft Length: 0.100000001
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /move_base/GlobalPlanner/plan
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 0.699999988
|
||||
- Alpha: 0.699999988079071
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
|
@ -280,36 +276,39 @@ Visualization Manager:
|
|||
Hide Inactive Objects: true
|
||||
- Class: rviz/Select
|
||||
- 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/Measure
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Angle: -0.00579636358
|
||||
Angle: -1.5507957935333252
|
||||
Class: rviz/TopDownOrtho
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.00999999978
|
||||
Scale: 3.49905491
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Scale: 6.9065141677856445
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: TopDownOrtho (rviz)
|
||||
X: 2.63081074
|
||||
Y: -9.44457054
|
||||
X: 2.6308107376098633
|
||||
Y: -9.444570541381836
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1056
|
||||
Height: 1025
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000003dafc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000003da000000d700fffffffb0000000a0049006d0061006700650000000317000000cc0000000000000000fb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf000003da00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000016a000003a7fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000000a0049006d0061006700650000000317000000cc0000000000000000fb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000005cd000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
|
@ -318,6 +317,6 @@ Window Geometry:
|
|||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1855
|
||||
X: 65
|
||||
Y: 24
|
||||
Width: 1853
|
||||
X: 67
|
||||
Y: 27
|
||||
|
|
|
@ -7,7 +7,7 @@
|
|||
<arg name="pub_map_odom_transform" default="true"/>
|
||||
<arg name="scan_subscriber_queue_size" default="5"/>
|
||||
<arg name="scan_topic" default="/iris_0/scan"/>
|
||||
<arg name="map_size" default="2048"/>
|
||||
<arg name="map_size" default="512"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="odom_to_map_broadcaster" args="0 0 0 0 0 0 map odom 100" />
|
||||
|
||||
|
@ -26,8 +26,8 @@
|
|||
<!-- Map size / start point -->
|
||||
<param name="map_resolution" value="0.050"/>
|
||||
<param name="map_size" value="$(arg map_size)"/>
|
||||
<param name="map_start_x" value="0.5"/>
|
||||
<param name="map_start_y" value="0.5" />
|
||||
<param name="map_start_x" value="0.2"/>
|
||||
<param name="map_start_y" value="0.2" />
|
||||
<param name="map_multi_res_levels" value="2" />
|
||||
|
||||
<!-- Map update parameters -->
|
||||
|
@ -53,6 +53,7 @@
|
|||
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
|
||||
|
||||
<remap from="/slam_out_pose" to="/iris_0/pose"/>
|
||||
<!--remap from="/map" to="/map_new"/-->
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
|
||||
name="laser_scan_matcher_node" output="screen">
|
||||
<param name="fixed_frame" value = "odom"/>
|
||||
<param name="base_frame" value = "iris_0/base_link"/>
|
||||
<param name="base_frame" value = "base_link"/>
|
||||
<param name="max_iterations" value="10"/>
|
||||
<param name="use_imu" value="false"/>
|
||||
<param name="use_odom" value="false"/>
|
||||
|
|
|
@ -18,7 +18,7 @@ slam_gmapping
|
|||
<node pkg="laser_scan_matcher" type="laser_scan_matcher_node"
|
||||
name="laser_scan_matcher_node" output="screen">
|
||||
<param name="fixed_frame" value = "odom"/>
|
||||
<param name="base_frame" value = "iris_0/base_link"/>
|
||||
<param name="base_frame" value = "base_link"/>
|
||||
<param name="max_iterations" value="10"/>
|
||||
<param name="use_imu" value="false"/>
|
||||
<param name="use_odom" value="false"/>
|
||||
|
@ -29,7 +29,7 @@ slam_gmapping
|
|||
#### start gmapping ############################################
|
||||
|
||||
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
|
||||
<param name="base_frame" value='iris_0/base_link'/>
|
||||
<param name="base_frame" value='base_link/'/>
|
||||
<param name="map_udpate_interval" value="1.0"/>
|
||||
<param name="maxUrange" value="5.0"/>
|
||||
<param name="sigma" value="0.1"/>
|
||||
|
|
Loading…
Reference in New Issue