Mid level control code

Dependencies:   ros_lib_kinetic

Revision:
41:0e3898b29230
Parent:
37:37da606f4466