Explorar o código

将全局路径规划频率打开设置为1,可以解决路径规划时局部路径跟踪的全局路径错误问题

corvin rasp melodic %!s(int64=2) %!d(string=hai) anos
pai
achega
7b8474f72a

+ 2 - 2
src/robot_navigation/config/base_global_planner_params.yaml

@@ -15,7 +15,7 @@ base_global_planner: "global_planner/GlobalPlanner"
 GlobalPlanner:
     allow_unknown: false
     default_tolerance: 0.2
-    
+
     use_dijkstra: true
     use_quadratic: true
     use_grid_path: false
@@ -31,4 +31,4 @@ GlobalPlanner:
     orientation_mode: 1
     orientation_window_size: 1
 
-    outline_map: true
+    outline_map: true

+ 14 - 8
src/robot_navigation/config/dwa_local_planner_params.yaml

@@ -1,6 +1,7 @@
 # Description:DWA局部规划器配置参数,经过实际测试得到小车最小的x轴移动速度
 #   是0.05m/s,当小于该值小车可能无法响应移动命令.最小的旋转速度是0.4rad/s,
 #   当小于该值小车将无法响应小车的旋转命令.
+#   min_vel_x:设置为负值,可以使小车具备倒车移动功能;
 #   vy_samples:在y方向速度空间采样点数,对于差速驱动的小车y方向速度永远只有一个0.0m/s
 #   prune_plan:跟踪的全局路径参数,定义当机器人是否边沿着路径移动时,清除掉已经走过1m的全局路径;
 # Author: corvin
@@ -13,24 +14,25 @@ base_local_planner: "dwa_local_planner/DWAPlannerROS"
 
 DWAPlannerROS:
 # Robot configuration params
-  acc_lim_x: 0.7
-  acc_lim_y: 0
-  acc_lim_th: 1.8
-
-  max_vel_trans: 0.7
-  min_vel_trans: 0.1
-  trans_stopped_vel: 0.1
-
   max_vel_x: 0.7
   min_vel_x: -0.7
 
   max_vel_y: 0
   min_vel_y: 0
 
+# The velocity when robot is moving in a straight line
+  max_vel_trans: 0.7
+  min_vel_trans: 0.1
+  trans_stopped_vel: 0.1
+
   max_vel_theta: 2.0
   min_vel_theta: 0.4
   theta_stopped_vel: 0.4
 
+  acc_lim_x: 0.7
+  acc_lim_y: 0
+  acc_lim_th: 2.0
+
 # Goal Tolerance Parametes
   xy_goal_tolerance: 0.2
   yaw_goal_tolerance: 0.2
@@ -57,3 +59,7 @@ DWAPlannerROS:
 
 # Global plan parameters
   prune_plan: true
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true

+ 1 - 0
src/robot_navigation/config/global_costmap_params.yaml

@@ -17,6 +17,7 @@ global_costmap:
 
    update_frequency: 1.0
    publish_frequency: 1.0
+
    static_map: true
    rolling_window: false
    resolution: 0.05

+ 1 - 0
src/robot_navigation/config/local_costmap_params.yaml

@@ -17,6 +17,7 @@
 local_costmap:
     global_frame: map
     robot_base_frame: base_footprint
+
     update_frequency: 3.0
     publish_frequency: 3.0
 

+ 4 - 9
src/robot_navigation/launch/move_base.launch

@@ -1,15 +1,10 @@
 <!--
-  Description:
-    move base package load config file for slam,config file list below:
-    (1)costmap_common_params.yaml -global_costmap
-    (2)costmap_common_params.yaml -local_costmap
-    (3)local_costmap_params.yaml
-    (4)global_costmap_params.yaml
-    (5)base_local_planner_params.yaml
+  Description:move base软件包加载的配置参数,各参数意义如下:
     controller_frequency:move_base向控制底盘移动话题cmd_vel发送控制命令的频率.
   Author: corvin
   History:
     20190728:init this file.
+    20220922:设置planner_frequency参数为1,为1hz生成全局规划的路径.
 -->
 <launch>
   <node pkg="move_base" type="move_base" name="move_base" respawn="true" output="screen" clear_params="true">
@@ -17,13 +12,13 @@
     <param name="controller_patiente"  value="3.0"/>
 
     <param name="planner_patience"  value="5.0"/>
-    <param name="planner_frequency" value="0.0"/>
+    <param name="planner_frequency" value="1.0"/>
     <param name="conservative_reset_dist" value="3.0"/>
 
     <param name="recovery_behavior_enabled" value="true"/>
     <param name="clearing_rotation_allowed" value="true"/>
 
-    <param name="shutdown_costmaps"    value="true"/>
+    <param name="shutdown_costmaps"    value="false"/>
     <param name="oscillation_timeout"  value="10.0"/>
     <param name="oscillation_distance" value="0.2"/>