Added mutex guards to all ROS functions and making sure all iterate functions
trigger it appropriately.