Fixed the queue by using const_cast (it turns out that unlike normal queues, priority_queues only return const references (api oversight?)