RSB iceWing Plugins

#include "RsbRstDepthImageImportPlugin.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 <rsb/converter/Repository.h>

#include <rsb/converter/SerializationException.h>

#include <rsb/converter/ProtocolBufferConverter.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 <opencv/cv.h>

#include <rsc/runtime/Properties.h>

#include <rsb/converter/Repository.h>

#include <rsb/util/QueuePushHandler.h>

#include <rsb/converter/ConverterSelectionStrategy.h>

#include <rsb/converter/PredicateConverterList.h>

#include <rsb/converter/RegexConverterPredicate.h>

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

using namespace std;

using namespace rsc::runtime;

using namespace rsc::threading;

using namespace rsb;

using namespace rsb::util;

using namespace rsb::converter;

using namespace rsb::filter;

static void iw_free_img_kinect(void* data) {

        iw_img_free((iwImage*) data, IW_IMG_FREE_ALL);

}

static void render16bit(prevBuffer *b, guchar **planes, int width, int height) {

        prevDataImage img;

        iwImage i;

        iw_img_init(&i);

        i.planes = 1;

        i.type = IW_16U;

        i.width = width;

        i.height = height;

        i.ctab = IW_GRAY;

        i.data = planes;

        img.i = &i;

        img.x = 0;

        img.y = 0;

        prev_render_imgs(b, &img, 1, RENDER_CLEAR, width, height);

}

using namespace ICEWING;

namespace rsbrstdepthimageimport {

class iwImageDeleter {

public:

    void operator()(void *i) {

        iwImage *image = (iwImage *) i;

        iw_img_free(image, IW_IMG_FREE_ALL );

    }

};

RsbRstDepthImageImportPlugin::RsbRstDepthImageImportPlugin(char *name) : ICEWING::Plugin(name), factory(rsb::getFactory()),

                                                           imageQueue(new SynchronizedQueue<boost::shared_ptr<rst::vision::Image> >(1)) {

}

RsbRstDepthImageImportPlugin::~RsbRstDepthImageImportPlugin() {

}

int RsbRstDepthImageImportPlugin::InitOptions() {

        plug_observ_data(this, "start");

        window = prev_new_window(plug_name(this, ".input"), -1, -1, false, false);

            return 0;

}

//wireType for rst::vision::Image

template<typename WireType>

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

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

    converters.push_back(

            make_pair(

                    ConverterPredicatePtr(

                            new RegexConverterPredicate("^.rst.vision.Image$")),

                    typename Converter<WireType>::Ptr(

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

    return typename ConverterSelectionStrategy<WireType>::Ptr(

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

                    converters.end()));

}

void RsbRstDepthImageImportPlugin::help() {

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

            "Import rst::vision::SimpleXYZImages messages and convert to generic KinectDepth.\n"

            "\n"

            " Options:\n"

            "  -i <s>   input RSB scope\n"

            "  -o <s>   iceWing indentifier for Image data data to output\n", name,

            ICEWING_NAME);

    gui_exit(1);

}

void RsbRstDepthImageImportPlugin::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':

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

            break;

        case 'O':

            imgIdent = (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 input identifier specified with command line arguments" << endl;

                error = true;

        }

        if (error) {

        help();

        }

        //listener 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);

        }

        listener = rsb::getFactory().createListener(scope, config);

        boost::shared_ptr<QueuePushHandler<rst::vision::Image> > dh(

            new QueuePushHandler<rst::vision::Image>(imageQueue));

        boost::shared_ptr<Handler> h = boost::static_pointer_cast<Handler>(dh);

        listener->addHandler(h);

}

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

        if (string(ident) != "start") {

                assert(false);

                return true;

        }

        boost::shared_ptr<rst::vision::Image> newImage = imageQueue->pop();

        //create and draw icewing image from rst::vision::Image

        if (newImage->depth() == rst::vision::Image::DEPTH_16U) {

                iwImage* imgOut = iw_img_new_alloc(newImage->width(), newImage->height(), 1, IW_16U);

                memcpy(imgOut->data[0], newImage->data().c_str(), newImage->width() * newImage->height()*2);

                imgOut->ctab = IW_GRAY;

                render16bit(window, imgOut->data, imgOut->width, imgOut->height);

                prev_draw_buffer(window);

                plug_data_set(this, imgIdent.c_str(), imgOut, iw_free_img_kinect);

        } else if (newImage->depth() == rst::vision::Image::DEPTH_8U) {

                iwImage* imgOut = iw_img_new_alloc(newImage->width(), newImage->height(), 1, IW_8U);

                memcpy(imgOut->data[0], newImage->data().c_str(), newImage->width() * newImage->height());

                imgOut->ctab = IW_GRAY;

                prev_render(window, imgOut->data, imgOut->width, imgOut->height, imgOut->ctab);

                prev_draw_buffer(window);

                plug_data_set(this, imgIdent.c_str(), imgOut, iw_free_img_kinect);

        }

            return true;

}

}

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

    *append = TRUE;

    ICEWING::Plugin* newPlugin = new rsbrstdepthimageimport::RsbRstDepthImageImportPlugin(

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

    return newPlugin;

}