Ask Your Question
0

Getting costmap_2d to update with LaserScan data

asked 2014-07-08 18:37:32 -0500

lucasw gravatar image

updated 2014-07-08 18:39:57 -0500

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?

image description

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

image description

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

image description

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?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2014-07-08 18:46:17 -0500

lucasw gravatar image

updated 2014-07-08 19:36:19 -0500

It looks like I needed to have negate set to 1 (though FREE_SPACE = 0 in the costmap_2d header file), and then the space near the robot and origin becomes free space and it looks like my LaserScan does mark the obstacles. But why wasn't the raytrace through the LETHAL_OBSTACLE section that the robot was surrounded by getting cleared when negate was 0- because no obstacles from the static map are ever cleared?

edit flag offensive delete link more

Comments

Lucas is correct. Your static map is being loaded backwards.

David Lu gravatar imageDavid Lu ( 2014-07-09 10:54:01 -0500 )edit

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: 2014-07-08 18:37:32 -0500

Seen: 625 times

Last updated: Jul 08 '14