Have a look at the costmap_2d package if you'd like some help. The "costmap_2d.cpp" source file contains most of the raytracing source code used in populating the costmaps with observation data from laser scans and point clouds.