ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Costmap_2d detects obstacles at wrong height if base_link moves in z-axis relative to the odom frame

asked 2016-02-26 14:32:57 -0500

zacwitte gravatar image

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:

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2016-02-26 15:18:42 -0500

dornhege gravatar image

Costmap transforms measurements into the maps frame, so it does, what it's supposed to do. If you think about, what is happening, transforming the obstacle messages relative to the robot's z might solve the issue. But essentially I think you are in case 3) You have costmap_2d and put a 3d problem in that. That can't work in general. You can transform your 3d problem back to 2d or 2.5d, but that requires understanding, what you want in the 2d formulation from a 3d world.

edit flag offensive delete link more


That's true. I ended up setting two_d_mode = true in robot_localization and that solved my issue of ensuring there's no change in z.

zacwitte gravatar image zacwitte  ( 2016-02-28 17:25:35 -0500 )edit

Question Tools

1 follower


Asked: 2016-02-26 14:32:57 -0500

Seen: 550 times

Last updated: Feb 26 '16