From b6377c70569f9df0256dd67d10d3f3905330f802 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Thu, 2 Apr 2020 07:10:33 +0200
Subject: [PATCH] navigation stack tuning

---
 config/base_local_planner_params.yaml | 44 ++++++++++++++++++++++++---
 config/global_costmap_params.yaml     |  7 ++---
 config/local_costmap_params.yaml      |  1 -
 config/nav_close_to_obstacles.yaml    |  9 ++++++
 launch/move_base.launch               | 15 ++++++++-
 urdf/wild_thumper.urdf.xacro          |  6 ++--
 6 files changed, 67 insertions(+), 15 deletions(-)
 create mode 100644 config/nav_close_to_obstacles.yaml

diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml
index bc544b8..5b3515d 100644
--- 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 3e1fd1d..d44e361 100644
--- 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 29fc1bc..f5d3e38 100644
--- 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
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 5586989..33c5636 100644
--- 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 f50da66..95957e3 100644
--- 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>
-- 
2.39.5