Costmap_layers problem with plugins [closed]

edit

Hi, everyone!!

I am trying to create a new layer for costmap to be use in turtlebot, I follow the two tutorials for costmap_2d. 1.Configuring Layered Costmaps 2.Creating a New Layer

So my costmap layers ymal results in these:

1.costmap_common_params.yaml

max_obstacle_height: 0.60  # assume something like an arm is mounted on top of the robot
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]
robot_radius: 0.18
inflation_radius: 0.50


# voxel map configuration; z-voxels 0 are filled by bumpers and 1 by laser scan (kinect)
map_type: voxel
origin_z: 0.0
z_resolution: 0.2
z_voxels: 2
publish_voxel_map: false

observation_sources: scan bump

scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35}
bump: {data_type: PointCloud2, topic: mobile_base/sensors/bumper_pointcloud, marking: true, clearing: false, min_obstacle_height: 0.0, max_obstacle_height: 0.15}

2.global_costmap_params.yaml global_costmap:

   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 1.0
   publish_frequency: 0.5
   static_map: true
   transform_tolerance: 0.5

3.my_local_costmap_params.yaml

local_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
   width: 4.0
   height: 4.0
   resolution: 0.05
   transform_tolerance: 0.5
   plugins:
   - {name: footprint_layer,     type: "costmap_2d::FootprintLayer"}      
   - {name: obstacle_layer_my,   type: "costmap_2d::ObstacleLayer" }     
   - {name: inflation_layer_my,  type: "costmap_2d::InflationLayer"}      
   - {name: grid_layer,          type: "simple_layer_namespace::GridLayer"}     
     #name: simple_layer
     #type: "simple_layer_namespace::SimpleLayer"

obstacle_layer_my:
    observation_sources: scan bump
    scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35}
    bump: {data_type: PointCloud2, topic: mobile_base/sensors/bumper_pointcloud, marking: true, clearing: false, min_obstacle_height: 0.0, max_obstacle_height: 0.15}


inflation_layer_my:
    observation_sources: scan bump
    scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35}
    bump: {data_type: PointCloud2, topic: mobile_base/sensors/bumper_pointcloud, marking: true, clearing: false, min_obstacle_height: 0.0, max_obstacle_height: 0.15}


grid_layer:
    observation_sources: scan bump
    scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, min_obstacle_height: 0.25, max_obstacle_height: 0.35}
    bump: {data_type: PointCloud2, topic: mobile_base/sensors/bumper_pointcloud, marking: true, clearing: false, min_obstacle_height: 0.0, max_obstacle_height: 0.15}

My problem is after running the gazebo and the my_gmapping_demo.launch, and rviz of turtlebot navigation like this:

1. optirun roslaunch turtlebotraul_gazebo turtlebot_empty_world.launch
2. roslaunch turtlebotraul_gazebo my_gmapping_demo.launch
3. optirun roslaunch turtlebot_rviz_launchers view_navigation.launch

I try to navigate the turtlebot in rviz using "2D Nav Goal" the path of global planner is all good, circumvents the obstacles, but for the local planner the path is wrong, for him doesn't exist obstacles so he try to pass through the obstacles, because my layer simple_layer or grid_layer aren't saving costs in the map. So I don't know if the problem is the cpp files of the layers: simple_layer or grid_layer, or the way I put the plugins in my_local_costmap_params.yaml, because when I run the roslaunch turtlebotraul_gazebo my_gmapping_demo.launch I get this:

[ INFO] [1397036705.765097077, 34.590000000]: Laser is mounted upwards.
 -maxUrange 6 -maxUrange 8 -sigma     0.05 -kernelSize 1 -lstep 0.05 -lobsGain 3 -astep 0.05
 -srr 0.01 -srt 0.02 -str 0.01 -stt ...