Fixed the majority of merging bugs. There may be some persisting issues, or they might just be rviz queue size issues
Merge branch 'alec'