Added rviz config file for astar search
Modified launch file to work properly, print outputs, and use config_astar.rviz
Modified Astar.py. Partially working