Stores a nav_msgs::OccupancyGrid in a convenient opencv cv::Mat as binary map (free: 255, occupied: 0) and as distance map (distance to closest obstacle in meter)
Determining the heuristic value by the euclidean distance between two states, the expected step costs to get from one state to the other and the difference between the orientation of the two states multiplied by some cost factor. (NOTE: choosing this angular difference cost might overestimate the heuristic value.)