"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/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):
Any ideas on what's happening and how to fix it?
What do your TF frames look like?
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.
Can you please post your full costmap configuration?
Sure. Is that all the costmap yaml files?
Or just
rosparam get /something/local_costmap
Thanks for the tip. Splitting this into two comments since it's too big for one.