Extended usability for the MoveIt! and Beliefstate bridges

Mostly fixed handling of special situations. Also, added convenience functions, and improved general speed of the logging mechanisms.