From 0bf5df6648a8d2ad791e2c0f95b83524eaded03a Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sun, 21 Feb 2016 11:06:13 +0100
Subject: [PATCH] navigation stack tuning

---
 config/base_local_planner_params.yaml | 25 ++++++++++++++++++++++---
 config/costmap_exploration.yaml       | 20 ++++++++++++++++++++
 config/global_costmap_params.yaml     |  1 -
 config/local_costmap_params.yaml      |  6 +++---
 launch/exploration.launch             | 13 +++++++++++++
 launch/move_base.launch               |  2 ++
 6 files changed, 60 insertions(+), 7 deletions(-)
 create mode 100644 config/costmap_exploration.yaml
 create mode 100644 launch/exploration.launch

diff --git a/config/base_local_planner_params.yaml b/config/base_local_planner_params.yaml
index 212e2b6..93b53a4 100644
--- a/config/base_local_planner_params.yaml
+++ b/config/base_local_planner_params.yaml
@@ -5,11 +5,30 @@ TrajectoryPlannerROS:
   max_vel_theta: 0.4
   min_in_place_vel_theta: 0.4
 
-  acc_lim_x: 2.5
+  acc_lim_x: 10.07
   acc_lim_y: 0.0
-  acc_lim_theta: 0.3
+  acc_lim_theta: 12.00
 
   holonomic_robot: false
   meter_scoring: true
 
-  sim_granularity: 0.01
+  xy_goal_tolerance: 0.20
+  yaw_goal_tolerance: 0.3
+
+DWAPlannerROS:
+  acc_lim_x: 10.07
+  acc_lim_y: 0.0
+  acc_lim_th: 12.00
+
+  max_vel_x: 0.5
+  min_vel_x: 0.0 # no backward movement
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+  max_trans_vel: 0.5
+  min_trans_vel: 0.2
+  max_rot_vel: 0.4
+  min_rot_vel: 0.3
+
+  xy_goal_tolerance: 0.20
+  yaw_goal_tolerance: 0.3
diff --git a/config/costmap_exploration.yaml b/config/costmap_exploration.yaml
new file mode 100644
index 0000000..26f3800
--- /dev/null
+++ b/config/costmap_exploration.yaml
@@ -0,0 +1,20 @@
+#must match incoming static map
+global_frame: map
+robot_base_frame: base_link
+resolution: 0.05
+
+track_unknown_space: true
+rolling_window: false
+
+plugins: 
+- {name: static,           type: "costmap_2d::StaticLayer"}            
+- {name: explore_boundary, type: "frontier_exploration::BoundedExploreLayer"}
+#Can disable sensor layer if gmapping is fast enough to update scans
+- {name: sensor,           type: "costmap_2d::ObstacleLayer"}
+- {name: inflation,        type: "costmap_2d::InflationLayer"}
+
+explore_boundary:
+  resize_to_boundary: false
+  frontier_travel_point: middle
+  #set to false for gmapping, true if re-exploring a known area
+  explore_clear_space: false
diff --git a/config/global_costmap_params.yaml b/config/global_costmap_params.yaml
index 9ea3b6c..3015ff0 100644
--- a/config/global_costmap_params.yaml
+++ b/config/global_costmap_params.yaml
@@ -5,5 +5,4 @@ global_costmap:
   static_map: true
   plugins:
   - {name: static_layer, type: 'costmap_2d::StaticLayer'}
-  - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
diff --git a/config/local_costmap_params.yaml b/config/local_costmap_params.yaml
index 391e338..62b586d 100644
--- a/config/local_costmap_params.yaml
+++ b/config/local_costmap_params.yaml
@@ -5,12 +5,12 @@ local_costmap:
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
-  width: 2.0
-  height: 2.0
+  width: 3.0
+  height: 3.0
   resolution: 0.05
   plugins:
   - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'}
-  - {name: range_layer,   type: "range_sensor_layer::RangeSensorLayer"}
+  #- {name: range_layer,   type: "range_sensor_layer::RangeSensorLayer"}
   - {name: inflation_layer, type: 'costmap_2d::InflationLayer'}
 
   range_layer:
diff --git a/launch/exploration.launch b/launch/exploration.launch
new file mode 100644
index 0000000..a608265
--- /dev/null
+++ b/launch/exploration.launch
@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+	<node pkg="frontier_exploration" type="explore_client" name="explore_client" output="screen"/>
+
+	<node pkg="frontier_exploration" type="explore_server" name="explore_server" output="screen" >
+		<param name="frequency" type="double" value="1.0"/>
+		<!-- Should be less than sensor range -->
+		<param name="goal_aliasing" type="double" value="2.0"/>
+
+		<rosparam file="$(find wild_thumper)/config/costmap_common_params.yaml" command="load" ns="explore_costmap" />
+		<rosparam file="$(find wild_thumper)/config/costmap_exploration.yaml" command="load" ns="explore_costmap" />
+	</node>
+</launch>
diff --git a/launch/move_base.launch b/launch/move_base.launch
index 989cedc..a046e6e 100644
--- a/launch/move_base.launch
+++ b/launch/move_base.launch
@@ -3,6 +3,7 @@
 	<arg name="slam_gmapping" default="false" />
 	<arg name="map_file" default="$(find wild_thumper)/data/map.yaml" />
 	<arg name="nomap" default="false" />
+	<arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/> <!-- base_local_planner/TrajectoryPlannerROS || dwa_local_planner/DWAPlannerROS -->
 
 	<!-- Run AMCL -->
 	<group unless="$(arg nomap)">
@@ -27,6 +28,7 @@
 	</node>
 
 	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+		<param name="base_local_planner" value="$(arg base_local_planner)"/>
 		<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" unless="$(arg nomap)" />
-- 
2.39.2