Robotics StackExchange | Archived questions

"Creating a New Layer" Tutorial grid_layer Offset Issue

I am running the "Creating a New Layer" tutorial at http://wiki.ros.org/costmap_2d/Tutorials/Creating%20a%20New%20Layer, 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?

Asked by cmcheung on 2019-11-09 17:02:02 UTC

Comments

What do your TF frames look like?

Asked by David Lu on 2019-11-15 11:21:08 UTC

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.

Asked by cmcheung on 2019-11-15 14:50:19 UTC

Can you please post your full costmap configuration?

Asked by David Lu on 2019-11-15 15:29:49 UTC

Sure. Is that all the costmap yaml files?

Asked by cmcheung on 2019-11-17 10:43:53 UTC

Or just rosparam get /something/local_costmap

Asked by David Lu on 2019-11-18 09:52:01 UTC

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



  
Asked by cmcheung on 2019-11-18 14:32:39 UTC



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



  
Asked by cmcheung on 2019-11-18 14:32:57 UTC

Answers