Added removeAllCollisionObjects from planning scene monitor, publishCollisionSceneFromFile function, and formatting