用octomap生成的二维栅格地图进行move_base路径规划仿真_move_base栅格地图

1.map_server保存二维栅格地图

octomap生成的二维栅格地图可以用map_server保存

map_server是个功能包,这个功能包可以单独下载,也可以直接下载navigation包,navigation包里包括了map_server,建议直接下载navigation,因为后续路径规划仿真也要用到这个功能包.地址如下

https://github.com/ros-planning/navigation
1

我的是kenetic版本,注意别下载错了版本.

下载编译好之后,用命令

rosrun map_server map_saver map:=/projected_map -f xxx
1

因为octomap生成的二维栅格地图的话题是/projected_map

后边的xxx是文件名,可以自己随便改.

保存好之后会生成两个文件,一个xxx.pgm,一个xxx.yaml

要进行后续的路径规划仿真需要对yaml文件修改一下,点开xxx.yaml,具体内容如下:

image: xxx.pgm
resolution: 0.100000
origin: [-141.900006, -183.199994, -nan]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
1
2
3
4
5
6

把origin的z轴坐标-nan改成0.000000,不然之后用rbx1打不开地图,改好如下所示.

image: xxx.pgm
resolution: 0.100000
origin: [-141.900006, -183.199994, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

1
2
3
4
5
6
7

如果是Gmapping这种二维slam算法,应该是没有z轴坐标的,可能因为我建图用的是LIO-SAM,所以要把这个-nan改成0.000000.

2.move_base路径规划仿真

需要用到的包:
1.rbx1
2.arbotix
3.navigation(上一步装过就不用装了)

全部下载好并编译好了之后运行仿真

首先启动仿真节点:

 roslaunch rbx1_bringup fake_turtlebot.launch
1

然后启动定位节点

roslaunch rbx1_nav fake_amcl.launch map:=xxx.yaml
1

这里的xxx就是你之前的地图名字xxx.

最后启动rviz可视化节点:

rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz
1

move_base全局路径规划默认是用Dijkstra,但是可以改成A*.

3更改move_base全局路径规划为A*

(1)修改rbx1中的fake_move_base_amcl.launch,将其改为

<launch>
  <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />
 
    <rosparam file="$(find rbx1_nav)/config/fake/teb_local_planner_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/move_base_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/global_planner_params.yaml" command="load" />
 
<!--
     <param name="base_global_planner" value="global_planner/GlobalPlanner"/> 
     <param name="planner_frequency" value="1.0" />
     <param name="planner_patience" value="5.0" />
 
     <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
     <param name="controller_frequency" value="5.0" />
     <param name="controller_patience" value="15.0" />
-->
  </node>
  
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

(2)在config/fake/文件夹内添加3个yaml文件
1.move_base_prams.yaml

shutdown_costmaps: false
 
controller_frequency: 4.0
controller_patience: 3.0 # 3.0
 
# planner_frequency:全局规划操作的执行频率.如果设置为0.0,则全局规划器仅
# 在接收到新的目标点或者局部规划器报告路径堵塞时才会重新执行规划操作.
# planner_patience:在空间清理操作执行前,留给规划器多长时间来找出一条有效规划.
# planner_frequency: 1.0
# planner_patience: 3.0
planner_frequency: 1.0
planner_patience: 3.0
 
oscillation_timeout: 5.0
oscillation_distance: 0.2
 
# Planner selection
base_global_planner: "global_planner/GlobalPlanner"  
base_local_planner: "teb_local_planner/TebLocalPlannerROS"
 
max_planning_retries: 1
 
recovery_behavior_enabled: true
clearing_rotation_allowed: true
 
recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'aggressive_reset'
  #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
  #- name: 'super_reset'
  #  type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'clearing_rotation'
    type: 'rotate_recovery/RotateRecovery'
  #- name: 'move_slow_and_clear'
    #type: 'move_slow_and_clear/MoveSlowAndClear'
    
conservative_reset:
  reset_distance: 1.0
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]
 
aggressive_reset:
  reset_distance: 3.0
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]
 
super_reset:
  reset_distance: 5.0
  #layer_names: [static_layer, obstacle_layer, inflation_layer]
  layer_names: [obstacle_layer]
 
move_slow_and_clear:
  clearing_distance: 0.5
  limited_trans_speed: 0.1
  limited_rot_speed: 0.4
  limited_distance: 0.3
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

2.global_planner_params.yaml

GlobalPlanner:
  allow_unknown: false
  default_tolerance: 0.2
  visualize_potential: false
  use_dijkstra: true
  use_quadratic: true
  use_grid_path: false
  old_navfn_behavior: false
 
  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0
  publish_potential: true
  orientation_mode: 0
  orientation_window_size: 1
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

use_dijkstra：设置为true,将使用dijkstra算法,否则使用A*算法.

3.teb_local_planner_params.yaml

TebLocalPlannerROS:
 
 odom_topic: odom
 #odom_topic: /robot_pose_ekf/odom_combined
 map_frame: /odom
 #map_frame: /odom_combined
 
 # Trajectory
 
 teb_autosize: True
 dt_ref: 0.45
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 5
 
 # Robot
 
 max_vel_x: 0.3
 max_vel_x_backwards: 0.15
 max_vel_theta: 0.3
 acc_lim_x: 0.15
 acc_lim_theta: 0.15
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   #radius: 0.23 # for type "circular"
   #vertices: [[-0.1, -0.2], [-0.1, 0.2],[0.3, 0.2], [0.3, -0.2]]
   vertices: [[-0.1643, -0.243], [-0.1643, 0.243],[0.3207, 0.243], [0.3207, -0.243]]
 
 # GoalTolerance
 
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.5
 free_goal_vel: False
 
 # Obstacles
 
 min_obstacle_dist: 0.24
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 7
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 
 # Optimization
 
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 1
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 60
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 selection_alternative_time_cost: False # not in use yet
 
 # Homotopy Class Planner
 
 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77

完成之后,再按照第2节的move_base路径规划仿真的3个命令来运行就可以了.

(虽然改的是fake_move_base_amcl.launch,但是fake_amcl.launch文件中包含了fake_move_base_amcl的启动节点,而fake_move_base_amcl又包含了刚加的3个yaml文件)

这里我试了一下,如果只把use_dijkstra: true改成use_dijkstra: false的话,路径规划会出现如下问题:

[ERROR] [1620908547.508054664]: NO PATH!
[ERROR] [1620908547.509109119]: Failed to get a plan from potential when a legal potential was found. This shouldn't happen.
1
2

并且机器人不动.

这时候再把use_grid_path: false改成use_grid_path: true问题就解决了,具体为啥还不太清楚.