Fixed A*, should be working intelligently now.

Switched all of my debug messages to use rospy.logdebug and not rospy.logerr so we can switch into debug mode and still use them.