I have a basic implementation of A star running in C++, but when I want to trace back to the origin it fails
Node start_node = Node(x_robot,y_robot);