adds yocs_safety_controller package

Migration of kobuki_safety_controller
Wouldn't it be better to call it as just `SafetyController` since it is already under yocs_safty_controller?