visualisation improvements, and monitered navigation now in smach viewer

@Jailander 's visualisation stuff is also in this pull request because I've been merging it to my hydro-devel branch. It's a new package though, so it won't break anything