update_dynamic_reconfigure.cpp

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2010 by Markus Bader                                    *
 *   markus.bader@tuwien.ac.at                                             *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/



#include <tuw_uvc/uvc.h>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

extern "C" {
#include <libv4l2.h>
#include "luvcview/v4l2uvc.h"
#include <linux/videodev2.h>
}

void updateDynamicReconfigureFile(const std::string &filename, const std::vector<V4RCam::ControlEntryPtr > &controlEntries, V4RCam::FD fd)
{
    v4l2_queryctrl queryctrl;
    v4l2_control   control_s;
    memset(&queryctrl, 0, sizeof(queryctrl));
    memset(&control_s, 0, sizeof(control_s));
        std::string package_name("tuw_uvc");
    FILE *configfile;
    configfile = fopen(filename.c_str(), "w");
    fprintf(configfile, "#! /usr/bin/env python\n");
    fprintf(configfile, "#Autogenerated V4L Dynamic Control\n\n");
    fprintf(configfile, "PACKAGE='%s'\n", package_name.c_str());
    fprintf(configfile, "from dynamic_reconfigure.parameter_generator_catkin import *\n");
    fprintf(configfile, "gen = ParameterGenerator()\n\n");


    fprintf(configfile, "gen.add(\"show_camera_image\", bool_t, 0, \"Show camera image\", True)\n");
    fprintf(configfile, "gen.add(\"camera_freeze\", bool_t, 0, \"Pulbishes the last image over and over again\", True)\n");
    fprintf(configfile, "enum_convert_image = gen.enum([gen.const(\"raw_data\", int_t, 0, \"raw camera Data\"),");
    fprintf(configfile, "  gen.const(\"YUV422toRGB\", int_t, 1, \"converts image to rgb first\"),");
    fprintf(configfile, "  gen.const(\"YUV422toBRG\", int_t, 2, \"converts image to bgr first\"),");
    fprintf(configfile, "  gen.const(\"YUV422toGray\", int_t, 3, \"converts image to gray first\")], \"Convert image\")\n");
    fprintf(configfile, "gen.add(\"convert_image_first\", int_t, 3, \"Convets the raw image first to an other format\", 1, 0, 3, edit_method=enum_convert_image)\n");

    
    fprintf(configfile, "\n#Autogenerated Controls\n\n");
    for(std::vector<V4RCam::ControlEntryPtr>::const_iterator it = controlEntries.begin(); it != controlEntries.end(); it++) {
        queryctrl.id = (*it)->queryctrl->id;

        if(0 == ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl)) {
            if(queryctrl.flags & V4L2_CTRL_FLAG_DISABLED) {
                continue;
            } else {
                if(queryctrl.type == V4L2_CTRL_TYPE_INTEGER) {
                    fprintf(configfile, "gen.add(\"%s\", int_t, 0, \"%s\", %i, %i, %i)\n",
                            (*it)->varName.c_str(), queryctrl.name, queryctrl.default_value, queryctrl.minimum, queryctrl.maximum);
                } else if(queryctrl.type == V4L2_CTRL_TYPE_BOOLEAN) {
                    fprintf(configfile, "gen.add(\"%s\", bool_t, 0, \"%s\", %s)\n",
                            (*it)->varName.c_str(), queryctrl.name, queryctrl.default_value ? "True" : "False");
                } else if(queryctrl.type == V4L2_CTRL_TYPE_MENU) {
                    fprintf(configfile, "enum_%s = gen.enum([", (*it)->varName.c_str());

                    for(int i = queryctrl.minimum; i <= queryctrl.maximum; i++) {
                        struct v4l2_querymenu qm;
                        qm.id = queryctrl.id;
                        qm.index = i;

                        boost::this_thread::sleep(boost::posix_time::milliseconds(10));
                        if(v4l2_ioctl(fd, VIDIOC_QUERYMENU, &qm) == 0) {
                            std::string menuEntryName = (const char *)qm.name;
                            std::transform(menuEntryName.begin(), menuEntryName.end(), menuEntryName.begin(), V4RCam::removeNonAlNum);
                            boost::algorithm::trim_left_if(menuEntryName, boost::algorithm::is_any_of("_"));
                            boost::algorithm::trim_right_if(menuEntryName, boost::algorithm::is_any_of("_"));
                            fprintf(configfile, "gen.const(\"%s\", int_t, %i, \"%s\")",
                                    menuEntryName.c_str(), i, (const char *)qm.name);
                        } else {
                            fprintf(configfile, "gen.const(\"Unkown%i\", int_t, %i, \"Unkown\")",
                                    i, i);
                        }
                        if(i != queryctrl.maximum) fprintf(configfile, ", ");
                    }
                    fprintf(configfile, "], \"%s\")\n", queryctrl.name);

                    fprintf(configfile, "gen.add(\"%s\", int_t, 0, \"%s\", %i, %i, %i, edit_method=enum_%s)\n",
                            (*it)->varName.c_str(), queryctrl.name, queryctrl.default_value, queryctrl.minimum, queryctrl.maximum, (*it)->varName.c_str());
                } else if(queryctrl.type == V4L2_CTRL_TYPE_BUTTON) {
                } else if(queryctrl.type == V4L2_CTRL_TYPE_INTEGER64) {
                } else if(queryctrl.type == V4L2_CTRL_TYPE_CTRL_CLASS) {
                } else if(queryctrl.type == V4L2_CTRL_TYPE_STRING) {
                } else if(queryctrl.type == V4L2_CTRL_TYPE_BITMASK) {
                } else {
                }
            }
        }
        boost::this_thread::sleep(boost::posix_time::milliseconds(10));
    }
    fprintf(configfile, "\nexit(gen.generate(PACKAGE, \"%s\", \"CameraParameters\"))\n", package_name.c_str());
    fflush(configfile);
    fclose(configfile);
    std::cout << "Wrote file :" << filename << std::endl;
    boost::this_thread::sleep(boost::posix_time::milliseconds(100));
}


int main(int argc, char **argv)
{
    namespace po = boost::program_options;
    std::string filename;
    std::string device;
    po::options_description desc("Allowed Parameters");
    desc.add_options()
    ("help", "get this help message")
    ("device,d", po::value<std::string>(&device)->default_value("/dev/video0"), "Video device")
    ("file,f", po::value<std::string>(&filename), "File to genrate: like cfg/CameraParameters.cfg");

    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, desc), vm);
    } catch(const std::exception &ex) {
        std::cout << desc << std::endl;;
        exit(1);
    }
    po::notify(vm);

    if(vm.count("help") || (vm.count("file") == 0))  {
        std::cout << desc << std::endl;
        exit(1);
    }

    std::cout << "File: " << filename << std::endl;
    V4RCam v4lCam;
    V4RCam::FD fd = v4lCam.initCamera(device);
    const std::vector<V4RCam::ControlEntryPtr > &controlEntries = v4lCam.detectControlEnties();
    for(unsigned int  i = 0; i  < controlEntries.size(); i++) {
        std::cout << controlEntries[i]->getQueryCtrlInfo() << std::endl;
    }
    boost::this_thread::sleep(boost::posix_time::milliseconds(100));
    updateDynamicReconfigureFile(filename, controlEntries, fd);
    boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
}