costmap_2d: how to publish local costmap to topic
I'm running costmap_2d with PointCloud2 feed from a 3D lidar as the only source. Lidar points are preliminary filtered, the ground is mostly segmented out. It is used as a singular source for the singular plugin of type ObstacleLayer.
As I'm using the output occupancy grid for use with home-made custom code, I would like to have a small portion of costmap near the robot position be published after every update as OccupancyGrid. I guess this is what local_costmap settings can be used for, right?
Well, false. The setup works the same way with and without additional YAML file for the local costmap. Even worse, I cannot get the full costmap to be published every step. The /costmap/costmap topic is being published once at the start, all the rest time only costmap/costmap_update are published. This is happening despite always_send_full_costmap: true in the config.
What and where should I write to get instant local occupancy grid published?
Below are my two YAML files and the portion in the launchfile where I run the costmapping node. I'm working in Xubuntu 16.04, ROS Kinetic.
in the launchfile:
<node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" output="screen" clear_params="true">
<rosparam file="$(find lidar_proc)/params/costmap/costmap_common_voxel_params.yaml" command="load" ns="costmap" />
<rosparam file="$(find lidar_proc)/params/costmap/local_costmap_quan_params.yaml" command="load" ns="costmap" />
<remap from="/cloud_in" to="/lidar/flt_out_cloud"/>
</node>
costmap_common_voxel_params.yaml:
transform_tolerance: 0.5,
robot_base_frame: base_link,
width: 200,
height: 200,
origin_x: -150,
origin_y: -50,
resolution: 0.2,
footprint:
- [-11.2, -3.2]
- [-11.2, 3.2]
- [0.2, 3.2]
- [0.2, -3.2],
update_frequency: 10.0,
static_map: false,
publish_frequency: 10.0,
max_obstacle_height: 3.0,
min_obstacle_height: -2,
obstacle_range: 70.0,
raytrace_range: 70.0,
always_send_full_costmap: true,
plugins:
- {name: obstacles, type: "costmap_2d::VoxelLayer"}
obstacles: {
z_resolution: 0.2,
z_voxels: 15,
origin_z: -2.0,
publish_voxel_map: true,
mark_threshold: 0.5,
enabled: true,
max_obstacle_height: 3.0,
min_obstacle_height: -2,
obstacle_range: 70.0,
raytrace_range: 70.0,
inflation_radius: 0.0,
combination_method: 1,
observation_sources: lidar_obstacles,
lidar_obstacles: {
sensor_frame: /Sensor,
data_type: PointCloud2,
topic: /cloud_in,
expected_update_rate: 5.0,
observation_persistence: 0.0,
max_obstacle_height: 3.0,
min_obstacle_height: -2,
marking: true,
clearing: false,
raytrace_range: 70.0,
obstacle_range: 70.0}
}
local_costmap_quan_params.yaml:
local_costmap:
global_frame: /map
robot_base_frame: /base_link
static_map: false
rolling_window: true
resolution: 0.2
transform_tolerance: 0.5
width: 20
height: 20
always_send_full_costmap: true,
plugins:
- {name: obstacles, type: "costmap_2d::ObstacleLayer"}