Costmap_2d detects obstacles at wrong height if base_link moves in z-axis relative to the odom frame
I set up move_base and costmap 2d using sensor data from a PointCloud2 message from our Velodyne VPL-16 laser. It generates a 3D Pointcloud and I'm using the default min_obstacle_height and max_obstacle_height params. I'm testing using a rosbag of sensor data as we manually drive the robot through our test site. The odometry says the robot is slowly moving down on the z-axis.
Whether that's accumulated error or true I'm actually not sure, but it seems that costmap 2d has some funny behavior if the z-axis changes at all. when sensor data comes in to costmap_2d it is translated from the sensor frame into the global frame (in this case set to odom) and the obstacle filtering is done there. That means if the robot's pose is at z = -6.5 the obstacle will be located at z=-6.5 in the global frame which is below the min_obstacle_height threshold of z=0.0.
This seems like wrong behavior to me. I imagine the min obstacle height should be relative to the root's frame, not to the global frame. I guess I can see 2 ways of working around this, but both sound hacky. 1) Somehow clamp the z-axis on all the odometry messages to 0 so move_base doesn't think the robot moves up and down. Or 2) update costmap_2d's code to filter obstacles relative to the robot's frame. or 3) I'm not understanding something correctly. What does the community recommend?
The code in question is here: https://github.com/ros-planning/navig...
