From a1bd09f6503d227bd72c86c10c4bc10725e5e365 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 18 Sep 2015 16:53:43 -0400
Subject: [PATCH] Azimut3 config: updated move_base parameters

---
 .../config/base_local_planner_params.yaml     |  6 +--
 .../config/costmap_common_params_2d.yaml      | 45 +++++++++++++++++++
 .../azimut3/config/global_costmap_params.yaml |  1 +
 .../azimut3/config/local_costmap_params.yaml  | 14 ++++++
 .../config/local_costmap_params_2d.yaml       |  2 +-
 .../config/local_costmap_params_3d.yaml       |  2 +-
 6 files changed, 65 insertions(+), 5 deletions(-)
 create mode 100644 launch/azimut3/config/costmap_common_params_2d.yaml
 create mode 100644 launch/azimut3/config/local_costmap_params.yaml

diff --git a/launch/azimut3/config/base_local_planner_params.yaml b/launch/azimut3/config/base_local_planner_params.yaml
index 687e93a5..f1ba8d08 100644
--- a/launch/azimut3/config/base_local_planner_params.yaml
+++ b/launch/azimut3/config/base_local_planner_params.yaml
@@ -12,7 +12,7 @@ TrajectoryPlannerROS:
   max_vel_theta: 0.5
   min_vel_theta: -0.5
   min_in_place_vel_theta: 0.25
-  holonomic_robot: true
+  holonomic_robot: false
 
   xy_goal_tolerance:  0.25
   yaw_goal_tolerance: 0.25
@@ -27,8 +27,8 @@ TrajectoryPlannerROS:
 
   meter_scoring: true
 
-  pdist_scale: 0.95 # The higher will follow more the global path.
-  gdist_scale: 0.2
+  pdist_scale: 0.8 # The higher will follow more the global path.
+  gdist_scale: 0.8
   occdist_scale: 0.01
   publish_cost_grid_pc: false
 
diff --git a/launch/azimut3/config/costmap_common_params_2d.yaml b/launch/azimut3/config/costmap_common_params_2d.yaml
new file mode 100644
index 00000000..d58e9003
--- /dev/null
+++ b/launch/azimut3/config/costmap_common_params_2d.yaml
@@ -0,0 +1,45 @@
+footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
+footprint_padding: 0.02
+#robot_radius: 0.38
+#robot_radius: ir_of_robot
+inflation_layer:
+  inflation_radius: 0.6 # 2xfootprint, it helps to keep the global planned path farther from obstacles
+transform_tolerance: 2
+
+obstacle_layer:
+  obstacle_range: 2.5
+  raytrace_range: 3
+  max_obstacle_height: 0.4
+  track_unknown_space: true
+
+  observation_sources: laser_scan_sensor point_cloud_sensorA point_cloud_sensorB
+
+  laser_scan_sensor: {
+    data_type: LaserScan,
+    topic: base_scan,
+    expected_update_rate: 0.2,
+    marking: true,
+    clearing: true
+  }
+
+  point_cloud_sensorA: {
+    sensor_frame: base_footprint,
+    data_type: PointCloud2,
+    topic: obstacles_cloud,
+    expected_update_rate: 0.5,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: 0.04
+  }
+
+  point_cloud_sensorB: {
+    sensor_frame: base_footprint,
+    data_type: PointCloud2,
+    topic: ground_cloud,
+    expected_update_rate: 0.5,
+    marking: false,
+    clearing: true,
+    min_obstacle_height: -1.0 # make usre the ground is not filtered
+  }
+
+
diff --git a/launch/azimut3/config/global_costmap_params.yaml b/launch/azimut3/config/global_costmap_params.yaml
index 8b6b87f2..fefe8b85 100644
--- a/launch/azimut3/config/global_costmap_params.yaml
+++ b/launch/azimut3/config/global_costmap_params.yaml
@@ -6,5 +6,6 @@ publish_frequency: 2
 always_send_full_costmap: false
 plugins:
    - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
+   - {name: obstacle_layer, type: "costmap_2d:ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
 
diff --git a/launch/azimut3/config/local_costmap_params.yaml b/launch/azimut3/config/local_costmap_params.yaml
new file mode 100644
index 00000000..87950d24
--- /dev/null
+++ b/launch/azimut3/config/local_costmap_params.yaml
@@ -0,0 +1,14 @@
+global_frame: odom
+robot_base_frame: base_footprint
+update_frequency: 2
+publish_frequency: 1
+rolling_window: true
+width: 3.0
+height: 3.0
+resolution: 0.025
+origin_x: 0
+origin_y: 0
+
+plugins:
+  - {name: obstacle_layer,   type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}
diff --git a/launch/azimut3/config/local_costmap_params_2d.yaml b/launch/azimut3/config/local_costmap_params_2d.yaml
index caf8064c..0c4b7519 100644
--- a/launch/azimut3/config/local_costmap_params_2d.yaml
+++ b/launch/azimut3/config/local_costmap_params_2d.yaml
@@ -1,4 +1,4 @@
-global_frame: map
+global_frame: odom
 robot_base_frame: base_footprint
 update_frequency: 2.0
 publish_frequency: 2.0
diff --git a/launch/azimut3/config/local_costmap_params_3d.yaml b/launch/azimut3/config/local_costmap_params_3d.yaml
index 52f0889a..ca56c63b 100644
--- a/launch/azimut3/config/local_costmap_params_3d.yaml
+++ b/launch/azimut3/config/local_costmap_params_3d.yaml
@@ -1,5 +1,5 @@
 local_costmap:
-  global_frame: map
+  global_frame: odom
   robot_base_frame: base_footprint
   update_frequency: 2.0
   publish_frequency: 2.0