Implemented basic raycasting for mst_astar imageToLaserScan