navigation stack tuning

diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml
index bc544b8b5b38f947fe3ed4336cec678194a7ea95..5b3515dd4cb4dd426f2f17039c18d110f89e795c 100644 (file)
--- a/config/base_local_planner_params.yaml
+++ b/config/base_local_planner_params.yaml
@@ -1,10 +1,11 @@
 TrajectoryPlannerROS:
-  min_vel_x: 0.2
+  min_vel_x: 0.05
   max_vel_x: 0.5
   min_vel_theta: -1.0
   max_vel_theta: 1.0
   min_in_place_vel_theta: 0.4
   max_rotational_vel: 1.0 # used by rotate recovery
+  escape_vel: 0.0 # Speed used for driving during escapes in meters/sec. Will move slowly even when the static map shows it as blocked
 
   acc_lim_x: 0.14
   acc_lim_y: 0.0
@@ -16,6 +17,8 @@ TrajectoryPlannerROS:
 
   xy_goal_tolerance: 0.15
   yaw_goal_tolerance: 0.1
+  # If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so.
+  latch_xy_goal_tolerance: true
 
   # The weighting for how much the controller should attempt to avoid obstacles, default: 0.01
   occdist_scale: 0.02
@@ -41,19 +44,50 @@ DWAPlannerROS:
   acc_lim_th: 6.00
 
   max_vel_x: 0.5
-  min_vel_x: 0.0 # no backward movement
+  min_vel_x: 0.1
   max_vel_y: 0.0
   min_vel_y: 0.0
 
   max_trans_vel: 0.5
-  min_trans_vel: 0.2
+  min_trans_vel: 0.1
   max_rot_vel: 1.0
   min_rot_vel: 0.4
 
-  xy_goal_tolerance: 0.1
-  yaw_goal_tolerance: 0.2
+  xy_goal_tolerance: 0.15
+  yaw_goal_tolerance: 0.1
 
   # The weighting for how much the controller should attempt to avoid obstacles, default: 0.01
   occdist_scale: 0.02
   # The weighting for how much the controller should stay close to the path it was given, default: 32.0
   path_distance_bias: 64.0
+
+PoseFollower:
+  holonomic: false
+  max_vel_lin: 0.5
+  max_vel_th: 1.0
+  min_vel_lin: 0.0
+  min_vel_th: 0.0
+  tolerance_trans: 0.15
+  tolerance_rot: 0.1
+  tolerance_timeout: 0.1
+  trans_stopped_velocity: 0.001
+  rot_stopped_velocity: 0.001
+  allow_backwards: false
+  # If true, turn in place to face the new goal instead of arching towards it
+  turn_in_place_first: false
+  # If turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location
+  max_heading_diff_before_moving: 0.175
+  # Gain factor for translation component of output velocities 0..20
+  k_trans: 1.0
+  # Gain factor for rotation component of output velocities: 0..20
+  k_rot: 2.0
+  collision_planner: # =TrajectoryPlannerROS
+    holonomic_robot: false
+    meter_scoring: true
+    xy_goal_tolerance: 0.15
+    yaw_goal_tolerance: 0.1
+    heading_scoring: false
+    heading_scoring_timestep: 0.3
+    oscillation_reset_dist: 0.1
+    sim_granularity: 0.05
+    angular_sim_granularity: 0.05

diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml
index 3e1fd1d764713d2cedf3883339e36494f4bcb447..d44e361103e1616feabc8749f35aedbde92154ce 100644 (file)
--- a/config/global_costmap_params.yaml
+++ b/config/global_costmap_params.yaml
@@ -1,17 +1,14 @@
 global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
-  static_map: true
   publish_frequency: 1.0
   plugins:
   - {name: static_layer, type: 'costmap_2d::StaticLayer'}
-  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
+  #- {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
 
   inflation_layer:
-    # low slope decay curve
-    inflation_radius: 1.75
-    cost_scaling_factor: 2.58
+    inflation_radius: 0.4 # plan distance around corners
 
   obstacle_layer:
     combination_method: 0 # overwrite

diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml
index 29fc1bc3a1110422437cfafd1ce69f80657841ef..f5d3e383438110f0e04f038567bd10e61520f126 100644 (file)
--- a/config/local_costmap_params.yaml
+++ b/config/local_costmap_params.yaml
@@ -3,7 +3,6 @@ local_costmap:
   robot_base_frame: base_footprint
   update_frequency: 5.0
   publish_frequency: 2.0
-  static_map: false
   rolling_window: true
   width: 3.0
   height: 3.0

diff --git a/config/nav_close_to_obstacles.yaml b/config/nav_close_to_obstacles.yaml
new file mode 100644 (file)
index 0000000..a433fd2
--- /dev/null
+++ b/config/nav_close_to_obstacles.yaml
@@ -0,0 +1,9 @@
+TrajectoryPlannerROS:
+  occdist_scale: 0.01
+  path_distance_bias: 1.2
+
+local_costmap:
+  resolution: 0.01
+
+global_costmap:
+  inflation_radius: 0.2

diff --git a/launch/move_base.launch b/launch/move_base.launch
index 55869892142fc79c96398b5cd4447336485f079d..33c5636f3cf9ffdb09eeb780b7ccc04440c5fe16 100644 (file)
--- a/launch/move_base.launch
+++ b/launch/move_base.launch
@@ -3,8 +3,12 @@
        <arg name="slam_gmapping" default="false" />
        <arg name="map_file" default="$(find wild_thumper)/data/map.yaml" />
        <arg name="global_odom" default="false" />
+       <arg name="close_to_obstacles" default="false" />
        <arg name="nomap" default="false" />
-       <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/> <!-- base_local_planner/TrajectoryPlannerROS || dwa_local_planner/DWAPlannerROS -->
+       <arg name="base_global_planner" default="navfn/NavfnROS"/> <!-- navfn/NavfnROS || carrot_planner/CarrotPlanner -->
+       <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/> <!-- base_local_planner/TrajectoryPlannerROS || dwa_local_planner/DWAPlannerROS || pose_follower/PoseFollower -->
+       <!-- default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]" -->
+       <arg name="recovery_behaviors" default="[{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]"/>
 
        <!-- Run AMCL -->
        <group unless="$(arg nomap)">
@@ -38,13 +42,22 @@
 
        <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
                <param name="base_local_planner" value="$(arg base_local_planner)"/>
+               <param name="base_global_planner" value="$(arg base_global_planner)"/>
                <param name="controller_frequency" value="10.0" />
+               <param name="max_planning_retries" value="3" />
+               <param name="clearing_rotation_allowed" value="false" />
+               <param name="GlobalPlanner/default_tolerance" value="0.2" />
+               <param name="conservative_reset/affected_maps" value="local" />
+               <param name="make_plan_clear_costmap" value="false" />
+               <param name="make_plan_add_unreachable_goal" value="false" />
+               <rosparam param="recovery_behaviors" subst_value="True">$(arg recovery_behaviors)</rosparam>
                <rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
                <rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
                <rosparam file="$(find wild_thumper)/config/global_costmap_params.yaml" command="load" />
                <rosparam file="$(find wild_thumper)/config/global_costmap_params_odom.yaml" command="load" if="$(arg global_odom)" />
                <rosparam file="$(find wild_thumper)/config/local_costmap_params.yaml" command="load" />
                <rosparam file="$(find wild_thumper)/config/base_local_planner_params.yaml" command="load" />
+               <rosparam file="$(find wild_thumper)/config/nav_close_to_obstacles.yaml" command="load" if="$(arg close_to_obstacles)" />
                <remap from="/cmd_vel" to="move_base/cmd_vel"/>
        </node>
 </launch>

diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index f50da66a00d57acdc1b1e9dc43582523eeb45b9b..95957e3c36e4d705d04bfc82b86983bc8fd2bfc8 100644 (file)
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -165,8 +165,8 @@
                <gazebo reference="${pos}_${side}_wheel">
                        <mu1 value="1.0"/>
                        <mu2 value="0.2"/>
-                       <kp value="1000000.0" />
-                       <kd value="1.0" />
+                       <kp value="1000000000000.0" />
+                       <kd value="0.0" />
                        <fdir1 value="0 1 0"/>
                        <minDepth>0.005</minDepth>
                </gazebo>
@@ -274,7 +274,7 @@
                                        </horizontal>
                                </scan>
                                <range>
-                                       <min>0.15</min>
+                                       <min>0.20</min>
                                        <max>16.0</max>
                                        <resolution>0.01</resolution>
                                </range>