Moved all locking to the environment and to the task_manager_lib
Merge branch 'adam_PID' of https://github.com/cedricpradalier/ros_kf_stack into adam_PID