How to use PointCloud from pedsim_sensors to update costmap obstacles

[nvhungv2k]

Hi, all
I have been trying to navigate pedsim_ros built-in diff-robot in environment created pedsim_similator
I have been able to navigate robot to desirable goal by navigation stack:

• I have recorded and placed at https://www.youtube.com/watch?v=d6atQ8QfVFs

But as you seen, robot can't avoid obstacles (men) because I think that local costmap didn't update obstacles
To update obstacles to local costmap, I have done as following:

• I don't use static map for global map
• In local_costmap_params.yaml, I place two layers obstacle_layer and inflation_layer, obstacle_layer updated from pedsim_people_sensor/point_cloud_local topic:

   plugins:
     - {name: obstacle_layer,          type: "costmap_2d::ObstacleLayer"}
     - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

   obstacle_layer:
     observation_sources: bump
     bump: {data_type: PointCloud , sensor_frame: odom, topic: pedsim_people_sensor/point_cloud_local, marking: true, clearing: true, min_obstacle_height: 0.0, max_obstacle_height: 3 }

Hint: Pls view full source code at: https://github.com/AMRobots/pedsim_teb

• I checked pedsim_people_sensor/point_cloud_local topic, it is ok and can view it in rviz

--
I already have tried to solve this issue by:

• Method 1: set min_obstacle_height to 0.0 cm as suggestion at here
• Method 2: convert pedsim_people_sensor/point_cloud_local(PointCloud) to PointCloud2 (by point_cloud_converter package), then convert PointCloud2 to LaserScan (by pointcloud_to_laserscan package), finally use LaserScan to update local costmap

With two methods I failed.
I don't know misunderstand or make mistakes at any place, pls suggest me the way to fix it.

[makokal]

Here is a good example of a config that uses these sensors with move_base

# use rolling window version of costmap, as local costmap is not static
rolling_window: true
static_map: false
publish_frequency: 10.0
update_frequency: 10.0
combination_method: 1

width: 5.0
height: 5.0
resolution: 0.05
global_frame: odom

plugins:
 - {name: obstacles, type: "costmap_2d::ObstacleLayer"}
 - {name: inflater, type: "costmap_2d::InflationLayer"}

obstacles:
  observation_sources: /pedsim_people_sensor/point_cloud_local /pedsim_obstacle_sensor/point_cloud_local
  /pedsim_people_sensor/point_cloud_local: 
    data_type: PointCloud
    sensor_frame: base_footprint
    marking: true 
    clearing: true 
    obstacle_range: 4 
    raytrace_range: 4 
    map_type: costmap
  /pedsim_obstacle_sensor/point_cloud_local: 
    data_type: PointCloud
    sensor_frame: base_footprint
    marking: true
    clearing: true
    obstacle_range: 4
    raytrace_range: 4
    map_type: costmap

inflater:
  observation_sources: /pedsim_people_sensor/point_cloud_local /pedsim_obstacle_sensor/point_cloud_local
  /pedsim_people_sensor/point_cloud_local: 
    data_type: PointCloud
    sensor_frame: base_footprint
    marking: true
    clearing: true
    obstacle_range: 4
    raytrace_range: 4
  /pedsim_obstacle_sensor/point_cloud_local: 
    data_type: PointCloud
    sensor_frame: base_footprint
    marking: true
    clearing: true
    obstacle_range: 4
    raytrace_range: 4

A similar one for a global map looks very similar.