Mid level control code

Dependencies:   ros_lib_kinetic

Revision:
41:0e3898b29230
Parent:
28:8e0c502c1a50