[ros-diffs] [cgutman] 41461: - Big debugging update - Also fix some little bugs and reduce code duplication