[ros-users] costmap_2d elevation questions

[Jack O'Quin]

On Tue, Sep 21, 2010 at 1:59 PM, Eitan Marder-Eppstein
<eitan at willowgarage.com> wrote:
> On Mon, Sep 20, 2010 at 7:45 PM, Jack O'Quin <jack.oquin at gmail.com> wrote:
>>
>> It may seem nonsensical to ask about the "height" of a 2D costmap, but
>> I am confused by what we saw today and could use some help making
>> sense of it and deciding what to do.
>>
>> As mentioned in an earlier message, I am experimenting with
>> costmap_2d_node to see if our autonomous vehicle can use it for sensor
>> fusion. It seems to work OK in stage simulation. We are running ROS
>> C-turtle on Ubuntu Karmic and Lucid.
>>
>> Today, we experimented with it on the actual car feeding it an
>> obstacle point cloud from our Velodyne 3D LIDAR. I can view the
>> PointCloud in rviz and it looks reasonable. But, when I view the
>> obstacle GridCells, none appear. However, rviz can display the unknown
>> GridCells (which is all of them). Strangely, they appear a couple of
>> hundred meters below the car and the obstacles. This offset looks
>> close to the actual elevation of our test area (209 meters above sea
>> level), so I am guessing the map may think it's at elevation z=0.
>
> This seems like a reasonable assumption. The navigation stack just publishes
> those markers in the global_frame of the costmap and let's rviz display them
> wherever, so this implies that, perhaps, your map frame is not correct in Z.
> Have you tried using rviz to look at tf itself? You should see the map and
> odometric frames where you expect them.

I guess they are where I should have expected them. I got confused by
the fact that the Odometry rviz display actually shows the /vehicle,
not the /odom frame. The tf from /odom to /vehicle is published by the
Applanix GPS node. It encodes x and y as easting and northing
coordinates relative to a 100km UTM grid, and z as altitude in meters
above sea level. So, the /odom frame is actually at sea level and
aligned with the UTM grid, not where the car is displayed.

As mentioned before, the /map frame is currently tied to the /odom
frame by an identity transform, so it is at sea level, too. (While we
plan to do GPS localization eventually, our current device is accurate
enough that it's not immediately necessary.)

>> Our GPS odometry source publishes the actual elevation for the
>> nav_msgs/Odometry message and /odom->/vehicle transform. There is
>> (currently) a static identity transform from /map to /odom (0,0,0
>> translation and 0,0,0,1 rotation quaternion). So, I expected the map
>> to appear at the same elevation as the vehicle and the odometry. I
>> can't be certain whether this is purely an rviz display issue or
>> inherent to the costmap, but I suspect the latter, because it would
>> explain why no obstacles appear in the map, they are all way above the
>> /costmap/max_obstacle_height parameter setting (2.7m). I did try
>> adding 209 to the max and min obstacle heights to see if that would
>> make a difference, and it did not.
>
> Looking at the frames should help here I'd think. Adding 209 to the max and
> min obstacle heights might not be enough if you're using the voxel grid
> underlying model for the costmap. I'd also expect you to have to set the
> origin_z parameter on the voxel grid:
> http://www.ros.org/wiki/costmap_2d#Map_type_parameters. The correct way to
> solve this, however, is probably to make sure that the global_frame of the
> costmap exists at the correct height to begin with.

We are not (yet) using the voxel grid, although I plan to experiment
with that, too.

But, what is the correct height in an outdoor environment? I could
force the odometry to start at z=0. But, then the altitude will vary
depending on where we start. What happens when the car goes up a hill?

I hate to lose the 3D information provided by GPS. Maybe we need to
publish two odometry messages and transforms: one for 3D and one
flattened to 2D. That seems like a horrible kludge, but I can't think
of another way to use these 2D tools effectively. The car is a ground
vehicle, tied to the surface of a 2D manifold. So, a 2D obstacle map
*does* make sense.

Costmap_2d is first place we've had to deal with this. Our 2007 Urban
Challenge code was pretty much entirely 2D. Converting to 3D
interfaces has been part of porting to ROS.

If someone knows a better solution, I would love to hear it.
-- 
 joq