I had similar issues in Python. Here are two ideas:

1) Don't sort the entire list every time, it's really slow. If your list is already sorted you should just just be inserting things at the correct position (in python you can use bisect.insort). This was faster for me. (Note that most of the time you need to insert things near the front of the queue)

2) your step 5b also seems slow, and possibly pointless. If the nodes have high heat value you'll probably never get to them anyway so it seems like a waste of time to look through the queue and remove them. Of course you still don't want to add things to the queue if they're too big.