Skip to content

Instantly share code, notes, and snippets.

@netshade

netshade/frame_decoder.c

Created February 2, 2022 19:04
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save netshade/867cef0c749ebb5624d9e0a0d1ff59f6 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save netshade/867cef0c749ebb5624d9e0a0d1ff59f6 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. netshade created this gist Feb 2, 2022.
    262 changes: 262 additions & 0 deletions frame_decoder.c
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,262 @@
    #include <gdnative_api_struct.gen.h>
    #include <libavcodec/avcodec.h>
    #include <libavutil/avutil.h>
    #include <libavformat/avformat.h>
    #include <libavutil/imgutils.h>
    #include <pthread.h>
    #include <stdio.h>
    #include <stdlib.h>
    #include <string.h>

    const godot_gdnative_core_api_struct *api = NULL;
    const godot_gdnative_ext_nativescript_api_struct *nativescript_api = NULL;

    void *sensor_decoder_constructor(godot_object *p_instance, void *p_method_data);
    void sensor_decoder_destructor(godot_object *p_instance, void *p_method_data, void *p_user_data);
    godot_variant sensor_decoder_get_data(godot_object *p_instance, void *p_method_data,
    void *p_user_data, int p_num_args, godot_variant **p_args);

    void godot_log(const wchar_t * message){
    godot_string s;
    api->godot_string_new_with_wide_string(&s, message, wcslen(message));
    api->godot_print(&s);
    api->godot_string_destroy(&s);
    }

    #define LOG(format, ...) do {\
    const wchar_t buf[512];\
    if(swprintf((wchar_t *)buf, sizeof(buf), format, ##__VA_ARGS__) <= 0){\
    godot_log(L"Error printing to log");\
    } else {\
    godot_log(buf);\
    }\
    } while(0);

    void GDN_EXPORT godot_gdnative_init(godot_gdnative_init_options *p_options) {
    api = p_options->api_struct;

    // Now find our extensions.
    for (int i = 0; i < api->num_extensions; i++) {
    switch (api->extensions[i]->type) {
    case GDNATIVE_EXT_NATIVESCRIPT: {
    nativescript_api = (godot_gdnative_ext_nativescript_api_struct *)api->extensions[i];
    }; break;
    default: break;
    }
    }
    }


    void GDN_EXPORT godot_gdnative_terminate(godot_gdnative_terminate_options *p_options) {
    api = NULL;
    nativescript_api = NULL;
    }

    void GDN_EXPORT godot_nativescript_init(void *p_handle) {
    godot_instance_create_func create = { NULL, NULL, NULL };
    create.create_func = &sensor_decoder_constructor;

    godot_instance_destroy_func destroy = { NULL, NULL, NULL };
    destroy.destroy_func = &sensor_decoder_destructor;

    nativescript_api->godot_nativescript_register_class(p_handle, "SENSORDECODER", "Reference",
    create, destroy);

    godot_instance_method get_data = { NULL, NULL, NULL };
    get_data.method = &sensor_decoder_get_data;

    godot_method_attributes attributes = { GODOT_METHOD_RPC_MODE_DISABLED };

    nativescript_api->godot_nativescript_register_method(p_handle, "SENSORDECODER", "get_data",
    attributes, get_data);
    }

    typedef struct user_data_struct {
    pthread_t decoder_thread;
    pthread_rwlock_t frame_lock;
    uint8_t * frame_data;
    size_t frame_size;
    size_t buf_size;
    godot_pool_byte_array frame;
    int stop_decoder;
    } user_data_struct;

    void * decoder_thread(void * decoder_args) {
    user_data_struct *user_data = (user_data_struct *) decoder_args;
    AVFormatContext *pFormatCtx = NULL;
    AVCodecContext *pCodecCtx = NULL;
    AVCodec *pCodec = NULL;
    AVCodecParameters *pCodecParams = NULL;
    AVFrame *pFrame = NULL;
    AVDictionary *optionsDict = NULL;
    AVPacket packet;
    int videoStream;
    int frameFinished;
    int stop;



    if(avformat_open_input(&pFormatCtx, "http://192.168.4.63:5000/stream/depth", NULL, NULL) !=0) {
    LOG(L"Couldn't open file");
    return NULL; // Couldn't open file
    }

    if(avformat_find_stream_info(pFormatCtx, NULL)<0) {
    LOG(L"Couldn't find stream information");
    return NULL;
    }

    // Find the first video stream
    videoStream = -1;
    for(int i=0; i<pFormatCtx->nb_streams; i++) {
    if(pFormatCtx->streams[i]->codecpar->codec_type == AVMEDIA_TYPE_VIDEO) {
    videoStream = i;
    pCodecParams = pFormatCtx->streams[i]->codecpar;
    break;
    }
    }
    if(videoStream == -1) {
    LOG(L"Couldn't find video stream");
    return NULL; // Didn't find a video stream
    }



    // Find the decoder for the video stream
    pCodec = avcodec_find_decoder(pCodecParams->codec_id);

    if(pCodec==NULL) {
    LOG(L"Unsupported codec!");
    return NULL;
    }

    // Get a pointer to the codec context for the video stream
    pCodecCtx = avcodec_alloc_context3(pCodec);
    if(pCodecCtx == NULL){
    LOG(L"Couldn't create codec context");
    return NULL;
    }

    if(avcodec_open2(pCodecCtx, pCodec, &optionsDict) < 0) {
    LOG(L"Couldn't open codec");
    return NULL;
    }


    pFrame = av_frame_alloc();// Allocate video frame
    stop = 0;
    // Read frames and save first five frames to disk
    while(stop == 0 && av_read_frame(pFormatCtx, &packet) >= 0) {
    if(pthread_rwlock_rdlock(&user_data->frame_lock) != 0){
    continue;
    }
    stop = user_data->stop_decoder;
    if(pthread_rwlock_unlock(&user_data->frame_lock) != 0){
    LOG(L"Could not unlock locked thread, bailing");
    break;
    }
    if(stop == 1){
    LOG(L"Exiting decoder thread");
    break;
    }
    // Is this a packet from the video stream?
    if(packet.stream_index == videoStream) {
    // Decode video frame
    avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, &packet);

    // Did we get a video frame?
    if(frameFinished)
    {
    if(pthread_rwlock_wrlock(&user_data->frame_lock) != 0){
    LOG(L"Could not lock to frame write, dropping frame");
    continue;
    }
    int copied = av_image_copy_to_buffer(user_data->frame_data, user_data->buf_size, pFrame->data, pFrame->linesize, pFrame->format, pFrame->width, pFrame->height, 1);
    if(copied < 0){
    LOG(L"Could not copy frame contents, dropping frame");
    memset(user_data->frame_data, 0, user_data->frame_size);
    continue;
    } else {
    user_data->frame_size = copied;
    }
    if(pthread_rwlock_unlock(&user_data->frame_lock) != 0){
    LOG(L"Could not unlock frame write after locked, bailing");
    break;
    }
    }
    }
    // Free the packet that was allocated by av_read_frame
    av_free_packet(&packet);
    }
    av_free(pFrame);// Free the video frame
    avcodec_close(pCodecCtx);// Close the codec
    avformat_close_input(&pFormatCtx);// Close the video file
    LOG(L"Decode finished");

    return NULL;
    }

    void *sensor_decoder_constructor(godot_object *p_instance, void *p_method_data) {
    user_data_struct * user_data = api->godot_alloc(sizeof(user_data_struct));
    user_data->buf_size = 1048576;
    user_data->frame_data = (uint8_t *) malloc(sizeof(uint8_t) * user_data->buf_size);
    user_data->frame_size = 0;
    api->godot_pool_byte_array_new(&user_data->frame);
    user_data->stop_decoder = 0;
    if(pthread_rwlock_init(&user_data->frame_lock, NULL) != 0){
    LOG(L"Lock failed to initialize");
    }
    if(pthread_create(&user_data->decoder_thread, NULL, decoder_thread, (void *)user_data) != 0){
    LOG(L"Thread start failed");
    } else {
    LOG(L"Thread start");
    }
    return user_data;
    }

    void sensor_decoder_destructor(godot_object *p_instance, void *p_method_data, void *p_user_data) {
    user_data_struct * user_data = (user_data_struct *) p_user_data;
    if(pthread_rwlock_wrlock(&user_data->frame_lock) != 0) {
    LOG(L"Could not acquire lock");
    } else {
    user_data->stop_decoder = 1;
    if(pthread_rwlock_unlock(&user_data->frame_lock) != 0){
    LOG(L"Could not unlock frame lock");
    }
    }
    if(pthread_join(user_data->decoder_thread, NULL) != 0){
    LOG(L"Could not safely exit decoder thread");
    }
    if(pthread_rwlock_destroy(&user_data->frame_lock) != 0){
    LOG(L"Could not destroy frame lock");
    }
    free(user_data->frame_data);
    api->godot_pool_byte_array_destroy(&user_data->frame);
    api->godot_free(p_user_data);
    LOG(L"Destroying");
    }

    godot_variant sensor_decoder_get_data(godot_object *p_instance, void *p_method_data,
    void *p_user_data, int p_num_args, godot_variant **p_args) {
    user_data_struct * user_data = (user_data_struct *) p_user_data;
    godot_variant ret;
    if(pthread_rwlock_rdlock(&user_data->frame_lock) != 0){
    LOG(L"Could not lock to frame read");
    // TODO
    }
    godot_int size = api->godot_pool_byte_array_size(&user_data->frame);
    if(size != user_data->frame_size){
    api->godot_pool_byte_array_resize(&user_data->frame, user_data->frame_size);
    }
    godot_pool_byte_array_write_access * write_access = api->godot_pool_byte_array_write(&user_data->frame);
    uint8_t * ptr = api->godot_pool_byte_array_write_access_ptr(write_access);
    memcpy(ptr, user_data->frame_data, user_data->frame_size);
    api->godot_pool_byte_array_write_access_destroy(write_access);
    if(pthread_rwlock_unlock(&user_data->frame_lock) != 0){
    LOG(L"Could not unlock frame read after locked, bailing");
    // TODO
    }
    api->godot_variant_new_pool_byte_array(&ret, &user_data->frame);

    return ret;
    }