Costmap stops tracking of 2dPointCloud

asked 2020-08-08 09:33:37 -0500

Flontis gravatar image

updated 2020-08-08 09:34:08 -0500

I have a 2dPointCloud representing the lanes in my environment for the self driving car. This 2dPointCloud shall be represented as an obstacle in my teb_local_planner.

It works, but only for the first few seconds. Then, the lane is still detected, also published, but it is not written into the global costmap as it should. (can't provide a screenshot, since I'm new to ROS, so you have to trust me :D )

My global_costmap_params.yaml looks like:

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 1.0
  publish_frequency: 1.0
  static_map: false
  rolling_window: false
  track_unknown_space: true

  transform_tolerance: 1.0
  plugins:
    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

And my costmap_params.yaml like:

#---standard pioneer footprint---
#---(in meters)---
#footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ]
footprint: [ [-0.1,-0.125], [0.5,-0.125], [0.5,0.125], [-0.1,0.125] ]


transform_tolerance: 1.0
map_type: costmap

obstacle_layer:
  enabled: true
  subscribe_to_updates: true
  obstacle_range: 3.0
  raytrace_range: 3.5
  track_unknown_space: false
  combination_method: 1
  observation_sources: laser_scan_sensor point_cloud_sensor_right
  laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}


  point_cloud_sensor_right: {
    sensor_frame: map,
    data_type: PointCloud2,
    topic: lane_detector/aggregator/global_grid_pcl,
    marking: true,
    clearing: false,
    min_obstacle_height: -10.0,
    observation_persistence: 10.0,
    expected_update_rate: 1.0,
  }

inflation_layer:
  enabled:              true
  cost_scaling_factor:  1.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.1  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  map_topic:            "/map"

So the 2DPointCloud is published to the topic lane_detector/aggregator/global_grid_pcl. And there is still data published, since I can monitor the 2DPointCloud in RVIZ and it's right. But if I want the data to be added as obstacle in the global_costmap, it only works for the first few meters and then only the 2DPointCloud is visible in RVIZ. The global_costmap is still visible, but the lanes marked as obstacles are missing.

edit retag flag offensive close merge delete