source: pacpussensors/trunk/Vislab/20150310_lib3dv-1.2.0/src/main.cc@ 149

/* 3dv-client/main.cc
 *
 * Copyright (C) 2013 VisLab
 *
 * This file is part of 3dv-client; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 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 Lesser General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 */

#include "disk_writer.h"
#include "display.h"
#include "utils.h"

#include <lib3dv/3dv.h>
#include "playback.h"

#include <boost/algorithm/string/replace.hpp>
#include <boost/asio/io_service.hpp>
#include <boost/asio/signal_set.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/asio/ip/udp.hpp>
#include <boost/asio/placeholders.hpp>
#include <boost/cast.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/thread.hpp>
#include <boost/uuid/uuid.hpp>
#include <boost/uuid/uuid_io.hpp>

#include <algorithm>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

#define VERSION 20150310

int main(int argc, char* argv[])
{   
    std::string address;
    std::vector<boost::asio::ip::address> addresses;
    unsigned int log_level;
    bool autonumber;
    std::vector<std::string> pathnames;
    data_file_format::types data_format;
#ifdef ARCHIVE
    archive_file_format::types archive_format;
    std::string archive_name;
#endif
    std::string device_params_file_name;
    std::vector<std::string> set_value;
    std::string get_value;
    boost::uuids::uuid device_guid;
    uint64_t timeout;
    std::vector<std::string> playback_value;
    
    boost::program_options::typed_value<unsigned int>* log_option = new boost::program_options::typed_value<unsigned int>(&log_level);
    boost::program_options::typed_value<std::string>* interface_option = new boost::program_options::typed_value<std::string>(&address);
    boost::program_options::typed_value<std::vector<std::string> >* pathnames_option = new boost::program_options::typed_value<std::vector<std::string> >(&pathnames);
    boost::program_options::typed_value<bool>* autonumber_option = new boost::program_options::typed_value<bool>(&autonumber);
    boost::program_options::typed_value<data_file_format::types>* data_format_option = new boost::program_options::typed_value<data_file_format::types>(&data_format);
#ifdef ARCHIVE
    boost::program_options::typed_value<archive_file_format::types>* archive_format_option = new boost::program_options::typed_value<archive_file_format::types>(&archive_format);
    boost::program_options::typed_value<std::string>* archive_name_option = new boost::program_options::typed_value<std::string>(&archive_name);
#endif
    boost::program_options::typed_value<std::string>* export_properties_option = new boost::program_options::typed_value<std::string>(&device_params_file_name);
    boost::program_options::typed_value<std::vector<std::string> >* set_value_option = new boost::program_options::typed_value<std::vector<std::string> >(&set_value);
    boost::program_options::typed_value<std::string>* get_value_option = new boost::program_options::typed_value<std::string>(&get_value);
    boost::program_options::typed_value<boost::uuids::uuid>* device_guid_option = new boost::program_options::typed_value<boost::uuids::uuid>(&device_guid);
    boost::program_options::typed_value<uint64_t>* timeout_option = new boost::program_options::typed_value<uint64_t>(&timeout);
    boost::program_options::typed_value<std::vector<std::string> >* playback_option = new boost::program_options::typed_value<std::vector<std::string> >(&playback_value);
    log_option->value_name("<level>");
    log_option->default_value(0U);
    
    interface_option->value_name("<address>");    
    
    pathnames_option->multitoken();
    pathnames_option->value_name("<paths>");
    pathnames_option->default_value(std::vector<std::string>(), "$GUID_$DATE-$TIME | .");
    
    autonumber_option->value_name("<enable>");
    autonumber_option->default_value(true, "true");
        
    data_format_option->value_name("{raw|binary|text|gnuplot}");
    data_format_option->default_value(data_file_format::TEXT, "text");
    
#ifdef ARCHIVE
    archive_name_option->default_value("$GUID_$DATE-$TIME");
    
    archive_format_option->value_name("{zip|tar}");
#ifdef __GNUC__
    archive_format_option->default_value(archive_file_format::TAR, "tar");
#else
    archive_format_option->default_value(archive_file_format::ZIP, "zip");
#endif
#endif

    export_properties_option->value_name("<file>");
    export_properties_option->default_value("$GUID_$DATE-$TIME.xml");
    
    set_value_option->multitoken();
    set_value_option->value_name("<address> <value> {bool|int64|int32|int16|int8|uint64|uint32|uint16|uint8|float32|float64|string}");
    
    get_value_option->value_name("<address>");
    
    device_guid_option->value_name("<guid>");
    
    timeout_option->value_name("<ms>");
    timeout_option->default_value(lib3dv::device::DEFAULT_TIMEOUT.total_milliseconds());

    playback_option->multitoken();
    playback_option->value_name("<path>");
    
    
    boost::program_options::options_description description("Options");

    description.add_options()
        ("help", "this help message")
        ("log", log_option, "log level")
        ("list-interfaces", "list the available network connections")
        ("interface", interface_option, "the IP address of the local interface to bind to")
        ("device-guid", device_guid_option, "target device GUID")
        ("record", "perform recording")
#ifdef PCL
        ("display", "display live data")
        ("playback",playback_option, "display recorded data in a given path")
#endif
        ("recording-paths", pathnames_option, "the recording path(s)")
        ("autonumber", autonumber_option, "append the frame number to the file name")
        ("data-format", data_format_option, "data files format")
#ifdef ARCHIVE
        ("archive", "use a single archive to store recorded files")
        ("archive-name", archive_name_option, "archive name")
        ("archive-format", archive_format_option, "archive file format")
#endif
        ("list-devices", "list devices")
        ("list-properties", "list device properties")
        ("export-properties", export_properties_option, "export device properties to an xml file")
        ("save-properties", "save property values on the device")
        ("reset-properties", "restore default property values on the device")
        ("set-value", set_value_option, "set a property value")
        ("get-value", get_value_option, "get a property value")
        ("poweroff", "shut down the device")
        ("reboot", "reboot the device")
        ("timeout", timeout_option, "network timeout")
        ("version", "program version");

    boost::program_options::variables_map variables_map;

    try
    {
        boost::program_options::store(boost::program_options::parse_command_line(argc, argv,
                                                                                 description,
                                                                                 boost::program_options::command_line_style::unix_style ^ boost::program_options::command_line_style::allow_short),
                                                                                 variables_map);
        boost::program_options::notify(variables_map);
        
        if(variables_map.count("help") || argc == 1)
        {
            std::cout << description << std::endl;
            return 0;
        }

        if(variables_map.count("version"))
        {
            std::cout << "3dv-client " << VERSION << " using lib3dv " << LIB3DV_VERSION_MAJOR << '.' << LIB3DV_VERSION_MINOR << '.' << LIB3DV_VERSION_PATCH << std::endl;
            std::cout <<  "Copyright (C) 2013 VisLab S.r.L.\nThis is free software; see the source for copying conditions.\nThere is NO warranty; not even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE." << std::endl << std::endl;

            return 0;
        }     
        
        if(variables_map.count("list-interfaces"))
        {
            addresses = list_interface_addresses();
            
            if(addresses.empty())
            {
                std::cerr << "[EE] 3dv-client: " << "no configured address found" << std::endl;

                return lib3dv::error::NETWORK_FAILURE;
            }

            std::cout << "[II] 3dv-client: available interfaces are:" << std::endl;
            
            for(unsigned int a = 0; a < addresses.size(); ++a)
                std::cout << "\t" << addresses[a].to_v4().to_string() << std::endl;
            
            return lib3dv::error::NONE;
        }
        
        if(!variables_map.count("interface"))
        {
            addresses = list_interface_addresses();
            
            if(addresses.empty())
            {
                std::cerr << "[EE] 3dv-client: " << "no configured address found" << std::endl;

                return lib3dv::error::NETWORK_FAILURE;
            }
        }
        else
            addresses.push_back(boost::asio::ip::address_v4::from_string(address));
    }
    catch(boost::program_options::error& e)
    {
        std::cerr << "[EE] 3dv-client: " << e.what() << std::endl;
        std::cout << description << std::endl;

        return lib3dv::error::NONE;
    }
    
    std::vector<lib3dv::device> devices;
    std::vector<lib3dv::device>::iterator device = devices.end();
    lib3dv::error error;

    if(variables_map.count("playback"))
    {
      return playback(playback_value,log_level);   
    }
    
    
    
    for(unsigned int a = 0; a < addresses.size(); ++a)
    {
        std::cout << "[II] 3dv-client: detecting devices on " << addresses[a].to_v4().to_string() << std::endl;
        
        std::vector<lib3dv::device> curr_devices = lib3dv::device::enumerate(addresses[a].to_v4(), log_level, error, boost::posix_time::milliseconds(timeout));
        
        if(error != lib3dv::error::NONE)
            std::cout << "[EE] 3dv-client: error detected: " << error << std::endl;
        else
            devices.insert(devices.end(), curr_devices.begin(), curr_devices.end());
    }
    
    if(devices.empty())
    {
        std::cout << "[EE] 3dv-client: no devices found" << std::endl;
        return lib3dv::error::NONE;
    }
        
    if(variables_map.count("timeout"))
    {
        if(!devices.empty())
            for(std::vector<lib3dv::device>::iterator dd = devices.begin(); dd != devices.end(); ++dd)
                dd->timeout(boost::posix_time::milliseconds(timeout));
    }
    
    if(variables_map.count("list-devices"))
    {
        for(std::vector<lib3dv::device>::iterator dd = devices.begin(); dd != devices.end(); ++dd)
            std::cout << '\t' << *dd << std::endl;

        return lib3dv::error::NONE;
    }
    
    if(variables_map.count("device-guid"))
    {
        for(std::vector<lib3dv::device>::iterator dd = devices.begin(); dd != devices.end(); ++dd)
            if(dd->guid() == device_guid)
            {
                device = dd;
                break;
            }
            
        if(device == devices.end())
        {
            std::cerr << "[EE] 3dv-client: device " << device_guid << " not found" << std::endl;

            return lib3dv::error::DEVICE_NOT_FOUND;
        }
    }
    else
    {
        device = devices.begin();
        
        if(devices.size() > 1)
            std::cout << "[WW] 3dv-client: multiple devices found, but --device-guid not specified" << std::endl;
    }
    
    std::cout << "[II] 3dv-client: selected device " << device->guid() << std::endl;
    
    if(variables_map.count("list-properties"))
    {
        std::vector<lib3dv::device::property> properties = device->enumerate_properties(error);
            
        if(error == lib3dv::error::NONE)
        {
            std::cout << "[II] 3dv-client: device properties:" << std::endl;

            for(unsigned int p = 0; p < properties.size(); ++p)
                std::cout << '\t' << properties[p] << std::endl;
        }
        else
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;

        return error;
    }
    
    if(variables_map.count("save-properties"))
    {
        device->save_properties(error);

        if(error != lib3dv::error::NONE)
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;
        else
            std::cout << "[EE] 3dv-client: operation completed" << std::endl;

        return error;
    }
    
    if(variables_map.count("reset-properties"))
    {
        device->reset_properties(error);

        if(error != lib3dv::error::NONE)
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;
        else
            std::cout << "[EE] 3dv-client: operation completed" << std::endl;

        return error;
    }
    
    if(variables_map.count("set-value"))
    {
        if(set_value.size() == 3)
        {
            std::stringstream ss;
            uint16_t address;
            const std::string& value = set_value[1];
            const std::string& type = set_value[2];
            
            ss << std::hex << set_value[0];
            ss >> address;

            std::cout << "[II] 3dv-client: setting property " << set_value[0] << " value to " << value << std::endl;
            
            if(type == "bool") device->set_property_value(address, boost::any(boost::lexical_cast<bool>(value)), error);
            else if(type == "int64") device->set_property_value(address, boost::any(boost::lexical_cast<int64_t>(value)), error);
            else if(type == "int32") device->set_property_value(address, boost::any(boost::numeric_cast<int32_t>(boost::lexical_cast<int64_t>(value))), error);
            else if(type == "int16") device->set_property_value(address, boost::any(boost::numeric_cast<int16_t>(boost::lexical_cast<int64_t>(value))), error);
            else if(type == "int8") device->set_property_value(address, boost::any(boost::numeric_cast<int8_t>(boost::lexical_cast<int64_t>(value))), error);
            else if(type == "uint64") device->set_property_value(address, boost::any(boost::lexical_cast<uint64_t>(value)), error);
            else if(type == "uint32") device->set_property_value(address, boost::any(boost::numeric_cast<uint32_t>(boost::lexical_cast<uint64_t>(value))), error);
            else if(type == "uint16") device->set_property_value(address, boost::any(boost::numeric_cast<uint16_t>(boost::lexical_cast<uint64_t>(value))), error);
            else if(type == "uint8") device->set_property_value(address, boost::any(boost::numeric_cast<uint8_t>(boost::lexical_cast<uint64_t>(value))), error);
            else if(type == "float32") device->set_property_value(address, boost::any(boost::lexical_cast<float>(value)), error);
            else if(type == "float64") device->set_property_value(address, boost::any(boost::lexical_cast<double>(value)), error);
            else if(type == "string") device->set_property_value(address, boost::any(value), error);
            else std::cout << "[EE] 3dv-client: unsupported data type " << type << std::endl;
            
            if(error != lib3dv::error::NONE)
                std::cout << "[EE] 3dv-client: device error " << error << std::endl;
            else
                std::cout << "[EE] 3dv-client: operation completed" << std::endl;

            return error;
        }
        else
        {
            std::cerr << "[EE] 3dv-client: exactly 3 arguments must be provided to --set-value" << std::endl;

            return lib3dv::error::NONE;
        }
    }
    
    if(variables_map.count("get-value"))
    {
        std::stringstream ss;
        uint16_t address;
        
        ss << std::hex << get_value;
        ss >> address;
        
        const boost::any& value = device->get_property_value(address, error);
        
        if(error != lib3dv::error::NONE)
        {
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;

            return error;
        }
        
        const std::type_info& type = value.type();
    
        std::cout << "[II] 3dv-client: property " << get_value << " value is ";
        
        if(type == typeid(bool)) std::cout << boost::any_cast<bool>(value) << std::endl;
        else if(type == typeid(int64_t)) std::cout << boost::any_cast<int64_t>(value) << std::endl;
        else if(type == typeid(int32_t)) std::cout << boost::any_cast<int32_t>(value) << std::endl;
        else if(type == typeid(int32_t)) std::cout << boost::any_cast<int32_t>(value) << std::endl;
        else if(type == typeid(int16_t)) std::cout << boost::any_cast<int16_t>(value) << std::endl;
        else if(type == typeid(int8_t)) std::cout << static_cast<int32_t>(boost::any_cast<int8_t>(value)) << std::endl;
        else if(type == typeid(uint64_t)) std::cout << boost::any_cast<uint64_t>(value) << std::endl;
        else if(type == typeid(uint32_t)) std::cout << boost::any_cast<uint32_t>(value) << std::endl;
        else if(type == typeid(uint32_t)) std::cout << boost::any_cast<uint32_t>(value) << std::endl;
        else if(type == typeid(uint16_t)) std::cout << boost::any_cast<uint16_t>(value) << std::endl;
        else if(type == typeid(uint8_t)) std::cout << static_cast<uint32_t>(boost::any_cast<uint8_t>(value)) << std::endl;
        else if(type == typeid(float)) std::cout << boost::any_cast<float>(value) << std::endl;
        else if(type == typeid(double)) std::cout << boost::any_cast<double>(value) << std::endl;
        else if(type == typeid(std::string)) std::cout << boost::any_cast<std::string>(value) << std::endl;
                    
        return lib3dv::error::NONE;
    }

    if(variables_map.count("record")
#ifdef PCL
        || variables_map.count("display")
#endif
    )
    {
        boost::asio::io_service sighandler_io_service;
        boost::asio::io_service::work sighandler_io_service_work(sighandler_io_service);
        
        boost::thread disk_writer_thread;
        boost::shared_ptr<disk_writer> disk_writer;
        
        device_params params;
        params.downsample=(uint32_t)2;
  
        std::vector<lib3dv::device::property> properties = device->enumerate_properties(error);
        
        if(error == lib3dv::error::NONE)
        {
            for(unsigned int p = 0; p < properties.size(); ++p)
            {
                const std::string& name = properties[p].m_name;
                const boost::any& value = properties[p].m_value;
                
                if(name == "calibration.u0")
                    params.intrinsics.m_u0 = boost::any_cast<double>(value);
                else if(name == "calibration.v0")
                    params.intrinsics.m_v0 = boost::any_cast<double>(value);
                else if(name == "calibration.ku")
                    params.intrinsics.m_ku = boost::any_cast<double>(value);
                else if(name == "calibration.kv")
                    params.intrinsics.m_kv = boost::any_cast<double>(value);
                else if(name == "calibration.x")
                    params.position.m_x = boost::any_cast<double>(value);
                else if(name == "calibration.y")
                    params.position.m_y = boost::any_cast<double>(value);
                else if(name == "calibration.z")
                    params.position.m_z = boost::any_cast<double>(value);
                else if(name == "calibration.yaw")
                    params.orientation.m_yaw = boost::any_cast<double>(value);
                else if(name == "calibration.pitch")
                    params.orientation.m_pitch = boost::any_cast<double>(value);
                else if(name == "calibration.roll")
                    params.orientation.m_roll = boost::any_cast<double>(value);
                else if(name == "calibration.baseline")
                    params.baseline = boost::any_cast<double>(value);
                else if(name == "depth mapping.downsample ratio")
                    params.downsample =boost::any_cast<uint32_t>(value);
                else if(name =="advanced.detection.area.step")
                        params.area_step = boost::any_cast<double>(value);
            }
        }
        else
        {
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;
            return error;
        }

        if(variables_map.count("record"))
        {
            if(pathnames.empty())
                pathnames.push_back(variables_map.count("archive") ? "." : "$GUID_$DATE-$TIME");
                
            std::vector<boost::filesystem::path> paths;

            // remove duplicates
            std::sort(pathnames.begin(), pathnames.end());
            pathnames.erase(std::unique(pathnames.begin(), pathnames.end()), pathnames.end());
            
            // expand variables
            std::stringstream guid_ss;
            guid_ss << device->guid();
            
            //Check 3DV application version
            uint32_t app_version=device->version().m_application[2]+device->version().m_application[1]*100+device->version().m_application[0]*10000;
            
            uint8_t guid_type=9;
  
            if (app_version <10100)  guid_type=7;
            else if (app_version <10200)  guid_type=8;
            
            for(unsigned int p = 0; p < pathnames.size(); ++p)
                boost::replace_all(pathnames[p], "$GUID", guid_ss.str());
            
            boost::posix_time::ptime timestamp(boost::posix_time::second_clock::local_time());    

            std::stringstream date_ss;
            boost::posix_time::time_facet* date_facet = new boost::posix_time::time_facet("%Y%m%d");
            date_ss.imbue(std::locale(date_ss.getloc(), date_facet));    
            date_ss << timestamp;

            for(unsigned int p = 0; p < pathnames.size(); ++p)
                boost::replace_all(pathnames[p], "$DATE", date_ss.str());
            
            std::stringstream time_ss;
            boost::posix_time::time_facet* time_facet = new boost::posix_time::time_facet("%H%M%S");
            time_ss.imbue(std::locale(time_ss.getloc(), time_facet));    
            time_ss << timestamp;

            for(unsigned int p = 0; p < pathnames.size(); ++p)
                boost::replace_all(pathnames[p], "$TIME", time_ss.str());
            
            paths.reserve(pathnames.size());

            for(unsigned int p = 0; p < pathnames.size(); ++p)
            {
                paths.push_back(boost::filesystem::path(pathnames[p]));
                paths.back().make_preferred();
            }
#ifdef ARCHIVE
            
            if(variables_map.count("archive"))
            {
                boost::replace_all(archive_name, "$GUID", guid_ss.str());
                boost::replace_all(archive_name, "$DATE", date_ss.str());
                boost::replace_all(archive_name, "$TIME", time_ss.str());
                
                disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, archive_name, archive_format, data_format, autonumber, params, guid_type, log_level));
            }
            else
#endif
                disk_writer = boost::shared_ptr< ::disk_writer>(new ::disk_writer(paths, data_format, autonumber, params, guid_type, log_level));

            disk_writer_thread = boost::thread(boost::bind(&::disk_writer::run, disk_writer.get()));

            //device->connect_image_callback(boost::bind(&::disk_writer::image_callback, disk_writer.get(), _1, _2));
            //device->connect_obstacles_callback(boost::bind(&::disk_writer::obstacles_callback, disk_writer.get(), _1, _2));
            //device->connect_terrain_callback(boost::bind(&::disk_writer::terrain_callback, disk_writer.get(), _1, _2));
            //device->connect_motion_callback(boost::bind(&::disk_writer::motion_callback, disk_writer.get(), _1, _2));
            device->connect_image_callback(boost::function<void(boost::shared_ptr< const lib3dv::image>, unsigned int)>(boost::bind(&::disk_writer::image_callback, disk_writer.get(), _1, _2)));
            device->connect_obstacles_callback(boost::function<void (boost::shared_ptr< const std::vector<lib3dv::obstacle> >,  unsigned int)>(boost::bind(&::disk_writer::obstacles_callback, disk_writer.get(), _1, _2)));
            device->connect_terrain_callback(boost::function<void(boost::shared_ptr< const lib3dv::terrain>, unsigned int)>(boost::bind(&::disk_writer::terrain_callback, disk_writer.get(), _1, _2)));
            device->connect_motion_callback( boost::function<void(boost::shared_ptr< const lib3dv::motion>, unsigned int)>(boost::bind(&::disk_writer::motion_callback, disk_writer.get(), _1, _2)));
            device->connect_classification_callback( boost::function<void(boost::shared_ptr< const lib3dv::classification >, unsigned int)>(boost::bind(&::disk_writer::classification_callback, disk_writer.get(), _1, _2)));
        }
        #ifdef PCL

        boost::thread display_thread;
        boost::shared_ptr<display> display;

        if(variables_map.count("display"))
        {
                
                display = boost::shared_ptr< ::display>(new ::display(params.intrinsics, params.position, params.orientation, params.baseline, params.downsample, params.area_step,log_level));
                display_thread = boost::thread(boost::bind(&::display::run, display.get()));

                //device->connect_image_callback(boost::bind(image_callback_display, _1, _2, boost::ref(*display), log_level));
                //device->connect_terrain_callback(boost::bind(terrain_callback_display, _1, _2, boost::ref(*display), log_level));
                //device->connect_obstacles_callback(boost::bind(obstacles_callback_display, _1, _2, boost::ref(*display), log_level));
                //device->connect_motion_callback(boost::bind(motion_callback_display, _1, _2, boost::ref(*display), log_level));
                device->connect_image_callback(boost::function<void(boost::shared_ptr<const lib3dv::image>, unsigned int)>(boost::bind(image_callback_display, _1, _2, boost::ref(*display), log_level)));
                device->connect_terrain_callback(boost::function<void(boost::shared_ptr<const lib3dv::terrain>, unsigned int)>(boost::bind(terrain_callback_display, _1, _2, boost::ref(*display), log_level)));
                device->connect_obstacles_callback(boost::function<void (boost::shared_ptr<const std::vector<lib3dv::obstacle> >,  unsigned int)>(boost::bind(obstacles_callback_display, _1, _2, boost::ref(*display), log_level)));
                device->connect_motion_callback(boost::function<void (boost::shared_ptr< const lib3dv::motion>,  unsigned int)>(boost::bind(motion_callback_display, _1, _2, boost::ref(*display), log_level)));
                device->connect_classification_callback(boost::function<void (boost::shared_ptr< const lib3dv::classification>,  unsigned int)>(boost::bind(classification_callback_display, _1, _2, boost::ref(*display), log_level)));
                device->connect_classification_callback(boost::function<void (boost::shared_ptr< const lib3dv::classification>,  unsigned int)>(boost::bind(classification_callback_display, _1, _2, boost::ref(*display), log_level)));
                //std::cout << "binded callback" <<std::endl;
        }
        
        device->connect_timeout_callback(boost::bind(timeout_callback, disk_writer, display, boost::ref(sighandler_io_service)));
        boost::asio::signal_set signals(sighandler_io_service, SIGINT, SIGTERM);
        signals.async_wait(boost::bind(signal_handler, boost::asio::placeholders::error, boost::asio::placeholders::signal_number, disk_writer, display, boost::ref(sighandler_io_service)));
#else

        device->connect_timeout_callback(boost::bind(timeout_callback, disk_writer, boost::ref(sighandler_io_service)));
        boost::asio::signal_set signals(sighandler_io_service, SIGINT, SIGTERM);
        signals.async_wait(boost::bind(signal_handler, boost::asio::placeholders::error, boost::asio::placeholders::signal_number, disk_writer, boost::ref(sighandler_io_service)));
#endif
        
        device->stop_transmission(error);
        
        if(error != lib3dv::error::NONE)
        {
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;

            return error;
        }
        
        device->start_transmission(error);
        
        if(error != lib3dv::error::NONE)
        {
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;

            return error;
        }
        
        sighandler_io_service.run();
        
        device->stop_transmission(error);
      
        if(disk_writer)
        {
            disk_writer->stop();
            disk_writer_thread.join();
        }
        
#ifdef PCL
        if(display)
        {
            display->stop();
            display_thread.join();
        }
#endif    

        std::cout << "[II] 3dv-client: operation completed" << std::endl;        
        
        return lib3dv::error::NONE;
    }

    if(variables_map.count("poweroff"))
    {
        device->poweroff(error);

        if(error != lib3dv::error::NONE)
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;
        else
            std::cout << "[EE] 3dv-client: operation completed" << std::endl;

        return error;
    }
    
    if(variables_map.count("reboot"))
    {
        device->reboot(error);

        if(error != lib3dv::error::NONE)
            std::cout << "[EE] 3dv-client: device error " << error << std::endl;
        else
            std::cout << "[EE] 3dv-client: operation completed" << std::endl;

        return error;
    }
    
    return lib3dv::error::NONE;
}