Tuning azimut3 navigation parameters

launch/azimut3/config/base_local_planner_params.yaml

@@ -3,24 +3,39 @@ TrajectoryPlannerROS:
   # Current limits based on AZ3 standalone configuration.
   acc_lim_x:  0.75
   acc_lim_y:  0.75
-  acc_lim_theta: 4.00
+  acc_lim_theta: 4
   # min_vel_x and max_rotational_vel were set to keep the ICR at
   # minimal distance of 0.48 m. 
   # Basically, max_rotational_vel * rho_min <= min_vel_x
-  max_vel_x:  0.700
+  max_vel_x:  0.7
   min_vel_x:  0.24
-  max_rotational_vel: 0.5
-  min_in_place_rotational_vel: 0.15
+  max_vel_theta: 0.5
+  min_vel_theta: -0.5
+  min_in_place_vel_theta: 0.25
+  holonomic_robot: true
 
-  xy_goal_tolerance:  0.20
-  yaw_goal_tolerance: 0.20
-
-  sim_time: 1.5
-  vtheta_samples: 30
+  xy_goal_tolerance:  0.25
+  yaw_goal_tolerance: 0.25
+  latch_xy_goal_tolerance: true
+  
+  # make sure that the minimum velocity multiplied by the sim_period is less than twice the tolerance on a goal. Otherwise, the robot will prefer to rotate in place just outside of range of its target position rather than moving towards the goal.
+  sim_time: 1.5 # set between 1 and 2. The higher he value, the smoother the path (though more samples would be required).
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.05
+  vx_samples: 12
+  vtheta_samples: 20
 
   meter_scoring: true
 
+  pdist_scale: 0.95 # The higher will follow more the global path.
+  gdist_scale: 0.2
+  occdist_scale: 0.01
+  publish_cost_grid_pc: false
+
+#move_base
+controller_frequency: 10.0 #The robot can move faster when higher.
+
 #global planner 
 NavfnROS:
-    allow_unknown: false
-    visualize_potential: true
+    allow_unknown: true
+    visualize_potential: false

launch/azimut3/config/costmap_common_params.yaml

@@ -1,9 +1,9 @@
-obstacle_range: 2.5
-raytrace_range: 3.0
 footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
-footprint_padding: 0.03
+footprint_padding: 0.02
+#robot_radius: 0.38
 #robot_radius: ir_of_robot
-inflation_radius: 0.55
+inflation_layer:
+  inflation_radius: 0.7 # 2xfootprint, it helps to keep the global planned path farther from obstacles
 transform_tolerance: 2
 
 

launch/azimut3/config/global_costmap_params.yaml

@@ -1,10 +1,10 @@
-global_costmap:
-  global_frame: map
-  robot_base_frame: base_footprint
-  update_frequency: 1
-  publish_frequency: 1
-  always_send_full_costmap: true
-  plugins: [
-    {name: static_layer, type: "rtabmap_ros::StaticLayer"},
-    {name: inflation_layer, type: "costmap_2d::InflationLayer"}
-  ]
+#global_costmap
+global_frame: map
+robot_base_frame: base_footprint
+update_frequency: 1
+publish_frequency: 2
+always_send_full_costmap: false
+plugins:
+   - {name: static_layer, type: "rtabmap_ros::StaticLayer"}
+   - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+

launch/azimut3/config/local_costmap_params_2d.yaml

@@ -1,33 +1,50 @@
-local_costmap:
-  global_frame: map
-  robot_base_frame: base_footprint
-  update_frequency: 2.0
-  publish_frequency: 2.0
-  static_map: false
-  rolling_window: true
-  width: 4.0
-  height: 4.0
-  resolution: 0.025
-  origin_x: -2.0
-  origin_y: -2.0
+global_frame: map
+robot_base_frame: base_footprint
+update_frequency: 2.0
+publish_frequency: 2.0
+rolling_window: true
+width: 4.0
+height: 4.0
+resolution: 0.025
+origin_x: -2.0
+origin_y: -2.0
 
-  #observation_sources: laser_scan_sensor
-  observation_sources: laser_scan_sensor point_cloud_sensor
+plugins:
+  - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer,  type: "costmap_2d::InflationLayer"}
+
+obstacle_layer:
+  obstacle_range: 2.5
+  raytrace_range: 3.0
+  max_obstacle_height: 0.4
+ 
+  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: false}
+    clearing: true
+  }
 
-  point_cloud_sensor: {
+  point_cloud_sensorA: {
     sensor_frame: base_footprint,
     data_type: PointCloud2, 
-    topic: openni_points, 
+    topic: obstacles_cloud, 
     expected_update_rate: 0.5, 
     marking: true, 
     clearing: true,
-    min_obstacle_height: 0.05,
-    max_obstacle_height: 0.4}
+    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
+  }