Ask Your Question
0

costmap_2d: how to publish local costmap to topic

asked 2018-02-07 17:48:28 -0600

teonik gravatar image

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"}
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2018-02-15 16:36:55 -0600

David Lu gravatar image

I don't think this is your root problem, but you have some problems with your params. Splitting the params into two files is mostly for people who use a global and local costmap. Since you're just using the costmap_node (which has only one costmap), you can just define one param file. The params in the second file are not loaded to the correct place, since they will be loaded to /costmap_node/costmap/local_costmap/global_frame (for example) which is one too many namespaces.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-02-07 17:48:28 -0600

Seen: 1,350 times

Last updated: Feb 15 '18