int32 current_setpoint_index
float32[5] current_setpoint # x,y,z,angle,take_picture