"Creating a New Layer" Tutorial grid_layer Offset Issue

asked 2019-11-09 16:02:02 -0600

cmcheung gravatar image

updated 2019-11-09 16:02:55 -0600

I am running the "Creating a New Layer" tutorial at http://wiki.ros.org/costmap_2d/Tutori..., and have it successfully running. However, the grid_layer is putting the obstacles in the incorrect place.

The relevant code from the example is as follows:

  double mark_x = robot_x + cos(robot_yaw), mark_y = robot_y + sin(robot_yaw);
  if(worldToMap(mark_x, mark_y, mx, my)){
    setCost(mx, my, LETHAL_OBSTACLE);
  }

This should notionally put an obstacle in front of the robot. However, when run with the Husky Gazebo code in RVIZ, the obstacle ends up appearing behind and to the right of the robot, as show in this image (the pink dot is the obstacle):

image description

Any ideas on what's happening and how to fix it?

edit retag flag offensive close merge delete

Comments

What do your TF frames look like?

David Lu gravatar image David Lu  ( 2019-11-15 10:21:08 -0600 )edit

Base_link is right on center mass of the robot.

Note that I add this to costmap_common yaml to recenter origin when using setcost and worldtomap: origin_x: -4.95 origin_y: -4.95

I'm just hoping to understand why I needed to do that in the first place.

cmcheung gravatar image cmcheung  ( 2019-11-15 13:50:19 -0600 )edit

Can you please post your full costmap configuration?

David Lu gravatar image David Lu  ( 2019-11-15 14:29:49 -0600 )edit

Sure. Is that all the costmap yaml files?

cmcheung gravatar image cmcheung  ( 2019-11-17 09:43:53 -0600 )edit

Or just rosparam get /something/local_costmap

David Lu gravatar image David Lu  ( 2019-11-18 08:52:01 -0600 )edit

Thanks for the tip. Splitting this into two comments since it's too big for one.

footprint: '[[-0.5,-0.33],[-0.5,0.33],[0.5,0.33],[0.5,-0.33]]'
footprint_padding: 0.01
global_frame: odom
grid_layer: {enabled: true}
height: 10
inflation: {cost_scaling_factor: 10.0, enabled: true, inflate_unknown: false, inflation_radius: 1.0}
obstacle_range: 5.5
obstacles_laser:
  combination_method: 1
  enabled: true
  footprint_clearing_enabled: true
  laser: {clearing: true, data_type: LaserScan, inf_is_valid: true, marking: true,
    topic: scan}
  max_obstacle_height: 2.0
  observation_sources: laser
  obstacle_range: 5.5
  raytrace_range: 6.0
origin_x: -4.95
origin_y: -4.95
cmcheung gravatar image cmcheung  ( 2019-11-18 13:32:39 -0600 )edit

plugins:
- {name: obstacles_laser, type: 'costmap_2d::ObstacleLayer'}
- {name: inflation, type: 'costmap_2d::InflationLayer'}
- {name: second_layer, type: 'simple_layer_namespace::SecondLayer'}
- {name: grid_layer, type: 'simple_layer_namespace::GridLayer'}
publish_frequency: 3.0
raytrace_range: 6.0
resolution: 0.05
robot_base_frame: base_link
robot_radius: 0.46
rolling_window: true
second_layer: {enabled: true}
static: {map_topic: /map, subscribe_to_updates: true}
transform_tolerance: 0.5
update_frequency: 4.0
width: 10
cmcheung gravatar image cmcheung  ( 2019-11-18 13:32:57 -0600 )edit
add a comment