Merge the refactor/sergio+ed branch for robot_smach_states