RSB iceWing Plugins

#include "RsbRstDepthImageExportPlugin.h"

#include <iostream>

#include <stdio.h>

#include <gui/Ggui.h>

#include <gui/Goptions.h>

#include <google/protobuf/message.h>

#include <boost/shared_ptr.hpp>

#include <stdexcept>

#include <rsc/runtime/Properties.h>

#include <rsb/converter/Repository.h>

#include <rsb/converter/SerializationException.h>

#include <rsb/converter/ProtocolBufferConverter.h>

#include <rsb/util/QueuePushHandler.h>

#include <rsb/converter/ConverterSelectionStrategy.h>

#include <rsb/converter/PredicateConverterList.h>

#include <rsb/converter/RegexConverterPredicate.h>

#include <gui/Gimage.h>

#include <main/grab.h>

#include "rst/vision/SimpleXYZImage.pb.h"

#include "rst/vision/Image.pb.h"

#include "rst/math/MatrixDouble.pb.h"

#include "rst/math/VectorDouble.pb.h"

#include "rst/vision/Image.pb.h"

using namespace std;

static void freeObject(void* data) {

        if (data!=NULL){

                char* d = (char*) data;

                delete[] d;

        }

}

using namespace ICEWING;

using namespace std;

using namespace ICEWING;

using namespace rsc::runtime;

using namespace rsc::threading;

using namespace rsb;

using namespace rsb::converter;

using namespace rsb::filter;

namespace rsbrstdepthimageexport {

RsbRstDepthImageExportPlugin::RsbRstDepthImageExportPlugin(char *name) :

        ICEWING::Plugin(name), factory(rsb::getFactory()) {

}

RsbRstDepthImageExportPlugin::~RsbRstDepthImageExportPlugin() {

}

int RsbRstDepthImageExportPlugin::InitOptions() {

    return 0;

}

//create WireType

template<typename WireType>

typename ConverterSelectionStrategy<WireType>::Ptr createConverterSelectionStrategy() {

    list<pair<ConverterPredicatePtr, typename Converter<WireType>::Ptr> > converters;

    converters.push_back(

            make_pair(

                    ConverterPredicatePtr(

                            new RegexConverterPredicate(".*")),

                    typename Converter<WireType>::Ptr(

                            new rsb::converter::ProtocolBufferConverter<rst::vision::Image>())));

    return typename ConverterSelectionStrategy<WireType>::Ptr(

            new PredicateConverterList<WireType>(converters.begin(),

                    converters.end()));

}

void RsbRstDepthImageExportPlugin::help() {

    fprintf(stderr, "\n%s plugin for %s, (c) 2014 by Niels Kroemker\n"

            "Convert KinectDepth images to generic RST and export rst::vision::Image messages .\n"

            "\n"

            " Options:\n"

            "  -i <s>   iceWing indentifier for Image data to read\n"

            "  -o <s>   export RSB scope\n", name,

            ICEWING_NAME);

    gui_exit(1);

}

void RsbRstDepthImageExportPlugin::Init(grabParameter *para, int argc, char **argv) {

    void *arg;

    char ch;

    int nr = 0;

    while (nr < argc) {

        ch = iw_parse_args(argc, argv, &nr, &arg, "-h:H --help:H"

                        "-i:Ir --input:Ir"

                        "-o:Or --output:Or");

        switch (ch) {

        case 'H':

        case '\0':

            help();

            break;

        case 'I':

            imgIdent = (char*) arg;

            break;

        case 'O':

            scope = rsb::Scope((char*) arg);

            break;

        default:

            fprintf(stderr, "Unknown character %c!\n", ch);

            help();

        }

    }

    bool error = false;

    if (imgIdent.empty()) {

        cerr << "No image input identifier specified with command line arguments"

                << endl;

        error = true;

    }

    if (scope == rsb::Scope()) {

        cerr << "No output identifier specified with command line arguments" << endl;

        error = true;

    }

    if (error) {

        help();

    }

        //informer configuration

        ParticipantConfig config = factory.getDefaultParticipantConfig();

            set < ParticipantConfig::Transport > transports = config.getTransports();

        for (set<ParticipantConfig::Transport>::const_iterator it =

                    transports.begin(); it != transports.end(); ++it) {

                ParticipantConfig::Transport& transport = config.mutableTransport(

                        it->getName());

                Properties options = transport.getOptions();

                options["converters"] = createConverterSelectionStrategy<string>();

                transport.setOptions(options);

        }

        informer = rsb::getFactory().createInformer<rsb::AnyType>(scope, config);

    plug_observ_data(this, imgIdent.c_str());

}

bool RsbRstDepthImageExportPlugin::Process(char *ident, plugData *data) {

        if (string(ident) != imgIdent) {

                assert(false);

                return true;

            }

        imgInput = (iwImage*) data->data;

        //Create rst::vision::Image

            rst::vision::Image rstImage;

        if(imgInput->type == IW_16U) {

                rstImage.set_depth(rst::vision::Image::DEPTH_16U);

        }else if(imgInput->type == IW_8U) {

                rstImage.set_depth(rst::vision::Image::DEPTH_8U);

        }

        rstImage.set_channels(1);

        rstImage.set_width(imgInput->width);

        rstImage.set_height(imgInput->height);

        if (imgInput->rowstride == 0) {

                rstImage.set_data_order(rst::vision::Image::DATA_SEPARATE);

        } else {

                rstImage.set_data_order(rst::vision::Image::DATA_INTERLEAVED);

        }

        rstImage.set_color_mode(rst::vision::Image::COLOR_GRAYSCALE);

        // write data in rst::vision::Image

        if(imgInput->type == IW_16U) {

                rstImage.set_data(imgInput->data[0],

                        imgInput->width * imgInput->height * imgInput->planes*2);

        } else if(imgInput->type == IW_8U) {

                rstImage.set_data(imgInput->data[0],

                        imgInput->width * imgInput->height * imgInput->planes);

        }

        //Publish rst::vision::Image

        google::protobuf::Message *output = (google::protobuf::Message*) &rstImage;

            informer->publish(

            boost::shared_ptr<google::protobuf::Message>(output,

                    rsc::misc::NullDeleter()), rsc::runtime::typeName(*output));

            return true;

}

}

extern "C" plugDefinition* plug_get_info(int instCount, bool* append) {

    *append = TRUE;

    ICEWING::Plugin* newPlugin = new rsbrstdepthimageexport::RsbRstDepthImageExportPlugin(

            g_strdup_printf("convertdepthrst%d", instCount));

    return newPlugin;

}