Costmap stops tracking of 2dPointCloud
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.