Fusing R200 and LIDAR data | SLAM | gmapping

The navigation stack supports obstacle avoidance from 3D point clouds out of the box. Here’s the tutorial:

http://wiki.ros.org/navigation/Tutorials/RobotSetup#Costmap_Configuration_.28local_costmap.29_.26_.28global_costmap.29

It’s better to ask this kind of question over at https://answers.ros.org/. If you encounter problems while following the tutorial, create a question over there and copy a link here, I’ll try to answer.