Node* start_node = new Node(calXyIndex(sx,min_x), calXyIndex(sy,min_y),0.0,-1); Node* goal_node = new Node(calXyIndex(gx,min_x), calXyIndex(gy,min_y),0.0,-1);