Getting costmap_2d to update with LaserScan data
I'd like to run costmap_2d and update a loaded static map with new sensor data, but I don't see any updates happening. I can see the latest LaserScan data in rviz on the /base_scan topic, but shouldn't the map be updated underneath it to show those squares as blocked?
This is my example_params.yaml which I pieced together from http://wiki.ros.org/costmap_2d/Tutori... and other sources:
global_frame: /map
robot_base_frame: base_link
footprint: [
# tail
[-0.325, -0.20],
[-0.325, 0.2],
# nose
[0.325, 0.2],
[0.34, 0.0],
[0.325, -0.2]
]
plugins:
- {name: static_map, type: "costmap_2d::StaticLayer"}
- {name: inflation, type: "costmap_2d::InflationLayer"}
- {name: obstacles, type: "costmap_2d::ObstacleLayer"}
publish_frequency: 1.0
update_frequency: 4.0
obstacles:
track_unkown_space: true,
observation_sources: base_scan
base_scan: {
data_type: LaserScan,
sensor_frame: base_link,
clearing: true,
marking: true,
topic: /base_scan,
observation_persistance: 15.0
}
I launch this like this:
<?xml version="1.0"?>
<launch>
<node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" output="screen" >
<rosparam file="$(find path_planning)/config/example_params.yaml" command="load" ns="costmap" />
</node>
<node name="cm_map_server" pkg="map_server" type="map_server" output="screen" args="$(find path_planning)/config/map.yaml" />
<node name="cm_rviz1" pkg="rviz" type="rviz" args="-d $(find path_planning)/config/costmap.rviz" />
<node name="cm_robot_static_tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 map base_link 100" />
</launch>
I then attempt to update the costmap with LaserScan data:
rostopic pub /base_scan sensor_msgs/LaserScan '{header:stamp: now, frame_id: base_link}, angle_min: 0.0, angle_max: 0.1, angle_increment: 0.1, time_increment: 0, scan_time: 0.0, range_min: 0.1, range_max: 3.0, ranges: [2.0, 1.4] }'
It looks like the base_scan is successfully connected to the costmap_node
And this is my map.yaml
image: map.pgm
resolution: 0.025
origin: [-6.000000, -6.000000, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
Am I wrong to expect that the /costmap_node/costmap/costmap is going to update in response to sensor data marking and clearing it, or is that the responsibility of something else?