You can also try increasing the queue limit, which is an argument to the Advertise function. For example, the ~/physics/contacts topic is advertised in ContactManager::Init with a queue size of 50:
  this->contactPub =