<commit_before>// Copyright 2016 Yahoo Inc. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include <vespa/fastos/fastos.h>
#include <iostream>
#include <string>
#include <cstdlib>

#include <boost/program_options.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/scope_exit.hpp>

#include <vespa/fastos/app.h>
#include <vespa/config-zookeepers.h>

#include <vespa/fileacquirer/config-filedistributorrpc.h>
#include <vespa/filedistribution/distributor/config-filedistributor.h>
#include <vespa/filedistribution/model/config-filereferences.h>

#include <vespa/filedistribution/distributor/filedistributortrackerimpl.h>
#include <vespa/filedistribution/distributor/filedownloadermanager.h>
#include <vespa/filedistribution/distributor/filedownloader.h>
#include <vespa/filedistribution/distributor/signalhandling.h>
#include <vespa/filedistribution/distributor/state_server_impl.h>

#include <vespa/filedistribution/model/filedistributionmodelimpl.h>
#include <vespa/filedistribution/rpc/filedistributorrpc.h>
#include <vespa/filedistribution/common/exception.h>
#include <vespa/filedistribution/common/componentsdeleter.h>

namespace {
const char* programName = "filedistributor";
}

#include <vespa/log/log.h>
LOG_SETUP(programName);

using namespace std::literals;

using namespace filedistribution;
using cloud::config::ZookeepersConfig;
using cloud::config::filedistribution::FiledistributorConfig;
using cloud::config::filedistribution::FiledistributorrpcConfig;

class FileDistributor : public config::IFetcherCallback<ZookeepersConfig>,
                        public config::IFetcherCallback<FiledistributorConfig>,
                        public config::IFetcherCallback<FiledistributorrpcConfig>,
                        public config::IGenerationCallback
{
    class Components {
        ComponentsDeleter _componentsDeleter;
    public:
        const std::shared_ptr<ZKFacade> _zk;
        const std::shared_ptr<FileDistributionModelImpl> _model;
        const std::shared_ptr<FileDistributorTrackerImpl> _tracker;
        const std::shared_ptr<FileDownloader> _downloader;
        const FileDownloaderManager::SP _manager;
        const FileDistributorRPC::SP _rpcHandler;
        const std::shared_ptr<StateServerImpl> _stateServer;

    private:
        std::thread _downloaderEventLoopThread;
        config::ConfigFetcher _configFetcher;

        template <class T>
        typename std::shared_ptr<T> track(T* component) {
            return _componentsDeleter.track(component);
        }

    public:
        Components(const Components &) = delete;
        Components & operator = (const Components &) = delete;

        Components(const std::shared_ptr<ExceptionRethrower>& exceptionRethrower,
                   const config::ConfigUri & configUri,
                   const ZookeepersConfig& zooKeepersConfig,
                   const FiledistributorConfig& fileDistributorConfig,
                   const FiledistributorrpcConfig& rpcConfig)
            :_zk(track(new ZKFacade(zooKeepersConfig.zookeeperserverlist, exceptionRethrower))),
             _model(track(new FileDistributionModelImpl(
                                     fileDistributorConfig.hostname,
                                     fileDistributorConfig.torrentport,
                                     _zk,
                                     exceptionRethrower))),
             _tracker(track(new FileDistributorTrackerImpl(_model))),
             _downloader(track(new FileDownloader(_tracker,
                                                  fileDistributorConfig.hostname,
                                                  fileDistributorConfig.torrentport,
                                                  boost::filesystem::path(fileDistributorConfig.filedbpath)))),
             _manager(track(new FileDownloaderManager(_downloader, _model))),
             _rpcHandler(track(new FileDistributorRPC(rpcConfig.connectionspec, _manager))),
             _stateServer(track(new StateServerImpl(fileDistributorConfig.stateport))),
             _downloaderEventLoopThread(std::bind(&FileDownloader::runEventLoop, _downloader.get())),
             _configFetcher(configUri.getContext())

        {
            _manager->start();
            _rpcHandler->start();

            _tracker->setDownloader(_downloader);
            _configFetcher.subscribe<FilereferencesConfig>(configUri.getConfigId(), _model.get());
            _configFetcher.start();
            updatedConfig(_configFetcher.getGeneration());
        }

        void updatedConfig(int64_t generation) {
            vespalib::ComponentConfigProducer::Config curr("filedistributor", generation);
            _stateServer->myComponents.addConfig(curr);
        }

        ~Components() {
            _configFetcher.close();
            //Do not waste time retrying zookeeper operations when going down.
            _zk->disableRetries();

            _downloader->close();
            _downloaderEventLoopThread.join();
        }

    };

    typedef std::lock_guard<std::mutex> LockGuard;
    std::mutex _configMutex;

    bool _completeReconfigurationNeeded;
    std::unique_ptr<ZookeepersConfig> _zooKeepersConfig;
    std::unique_ptr<FiledistributorConfig> _fileDistributorConfig;
    std::unique_ptr<FiledistributorrpcConfig> _rpcConfig;

    std::shared_ptr<ExceptionRethrower> _exceptionRethrower;
    std::unique_ptr<Components> _components;
public:
    FileDistributor(const FileDistributor &) = delete;
    FileDistributor & operator = (const FileDistributor &) = delete;
    FileDistributor()
        : _configMutex(),
          _completeReconfigurationNeeded(false),
          _zooKeepersConfig(),
          _fileDistributorConfig(),
          _rpcConfig(),
          _exceptionRethrower(),
          _components()
    { }

    void notifyGenerationChange(int64_t generation) {
        if (_components && ! completeReconfigurationNeeded()) {
            _components->updatedConfig(generation);
        }
    }

    //configure overrides
    void configure(std::unique_ptr<ZookeepersConfig> config) {
        LockGuard guard(_configMutex);
        _zooKeepersConfig = std::move(config);
        _completeReconfigurationNeeded = true;
    }

    void configure(std::unique_ptr<FiledistributorConfig> config) {
        LockGuard guard(_configMutex);
        if (_fileDistributorConfig.get() != NULL &&
            (config->torrentport != _fileDistributorConfig->torrentport ||
             config->stateport   != _fileDistributorConfig->stateport ||
             config->hostname    != _fileDistributorConfig->hostname ||
             config->filedbpath  != _fileDistributorConfig->filedbpath))
        {
            _completeReconfigurationNeeded = true;
        } else if (_components.get()) {
            configureSpeedLimits(*config);
        }
        _fileDistributorConfig = std::move(config);

    }

    void configure(std::unique_ptr<FiledistributorrpcConfig> config) {
        LockGuard guard(_configMutex);
        _rpcConfig = std::move(config);
        _completeReconfigurationNeeded = true;
    }

    void run(const config::ConfigUri & configUri) {
        while (!askedToShutDown()) {
            clearReinitializeFlag();
            _exceptionRethrower.reset(new ExceptionRethrower());
            runImpl(configUri);

            if (_exceptionRethrower->exceptionStored())
                _exceptionRethrower->rethrow();
        }
    }

    static void ensureExceptionsStored(const std::shared_ptr<ExceptionRethrower>& exceptionRethrower) {
        //TODO: this is somewhat hackish, refactor to eliminate this later.
        LOG(debug, "Waiting for shutdown");
        for (int i=0; i<50 && !exceptionRethrower.unique(); ++i) {
            std::this_thread::sleep_for(100ms);
        }
        LOG(debug, "Done waiting for shutdown");

        if (!exceptionRethrower.unique()) {
            EV_STOPPING(programName, "Forced termination");
            kill(getpid(), SIGKILL);
        }
    }

    void createComponents(const std::shared_ptr<ExceptionRethrower>& exceptionRethrower, const config::ConfigUri & configUri) {
        LockGuard guard(_configMutex);
        _components.reset(
                new Components(exceptionRethrower,
                        configUri,
                        *_zooKeepersConfig,
                        *_fileDistributorConfig,
                        *_rpcConfig));

        configureSpeedLimits(*_fileDistributorConfig);
        _completeReconfigurationNeeded = false;
    }

    bool completeReconfigurationNeeded() {
        LockGuard guard(_configMutex);
        if (_completeReconfigurationNeeded) {
            LOG(debug, "Complete reconfiguration needed");
        }
        return _completeReconfigurationNeeded;
    }

    void configureSpeedLimits(const FiledistributorConfig& config) {
        FileDownloader& downloader = *_components->_downloader;
        downloader.setMaxDownloadSpeed(config.maxdownloadspeed);
        downloader.setMaxUploadSpeed(config.maxuploadspeed);
    }

    //avoid warning due to scope exit macro
#pragma GCC diagnostic ignored "-Wshadow"
    void runImpl(const config::ConfigUri & configUri) {

        BOOST_SCOPE_EXIT((&_components)(&_exceptionRethrower)) {
            _components.reset();
            //Ensures that any exception stored during destruction will be available when returning.
            ensureExceptionsStored(_exceptionRethrower);
        } BOOST_SCOPE_EXIT_END

        createComponents(_exceptionRethrower, configUri);

        // We do not want back to back reinitializing as it gives zero time for serving
        // some torrents.
        int postPoneAskedToReinitializedSecs = 50;

        while (!askedToShutDown() &&
	       (postPoneAskedToReinitializedSecs > 0 || !askedToReinitialize()) &&
	       !completeReconfigurationNeeded() &&
	       !_exceptionRethrower->exceptionStored()) {
	  postPoneAskedToReinitializedSecs--;
	  std::this_thread::sleep_for(1s);
        }
    }
};

//TODO: use pop in gcc 4.6
#pragma GCC diagnostic warning "-Wshadow"

class FileDistributorApplication : public FastOS_Application {
    const config::ConfigUri _configUri;
public:
    FileDistributorApplication(const config::ConfigUri & configUri);

    //overrides
    int Main();
};

namespace {

struct ProgramOptionException {
    std::string _msg;
    ProgramOptionException(const std::string & msg)
        : _msg(msg)
    {}
};

bool exists(const std::string& optionName, const boost::program_options::variables_map& map) {
    return map.find(optionName) != map.end();
}

void ensureExists(const std::string& optionName, const boost::program_options::variables_map& map \
                  ) {
    if (!exists(optionName, map)) {
        throw ProgramOptionException("Error: Missing option " + optionName);
    }
}

} //anonymous namespace

FileDistributorApplication::FileDistributorApplication(const config::ConfigUri & configUri)
    :_configUri(configUri) {
}

int
FileDistributorApplication::Main() {
    try {
        FileDistributor distributor;

        config::ConfigFetcher configFetcher(_configUri.getContext());
        configFetcher.subscribe<ZookeepersConfig>(_configUri.getConfigId(), &distributor);
        configFetcher.subscribe<FiledistributorConfig>(_configUri.getConfigId(), &distributor);
        configFetcher.subscribe<FiledistributorrpcConfig>(_configUri.getConfigId(), &distributor);
        configFetcher.subscribeGenerationChanges(&distributor);
        configFetcher.start();

        distributor.run(_configUri);

        EV_STOPPING(programName, "Clean exit");
        return 0;
    } catch(const FileDoesNotExistException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 1;
    } catch(const ZKNodeDoesNotExistsException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 2;
    } catch(const ZKSessionExpired & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 3;
    } catch(const config::ConfigTimeoutException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 4;
    } catch(const FailedListeningException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 5;
    } catch(const ZKGenericException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 99;
    } catch(const boost::unknown_exception & e) {
        std::string s = boost::diagnostic_information(e);
        LOG(warning, "Caught '%s'", s.c_str());
        EV_STOPPING(programName, s.c_str());
        return 255;
#if 0
    /*
     These are kept hanging around for reference as to how it was when we just held our ears
     singing "na, na, na, na..." no matter if the sun was shining or if the world imploded.
    */
    } catch(const boost::exception& e) {
        std::string s = boost::diagnostic_information(e);
        LOG(error, "Caught '%s'", s.c_str());
        EV_STOPPING(programName, s.c_str());
        return -1;
    } catch(const std::string& msg) {
        std::string s = "Error: " + msg;
        LOG(error, "Caught '%s'", s.c_str());
        EV_STOPPING(programName, s.c_str());
        return -1;
#endif
    }
}

int
executeApplication(int argc, char** argv) {
    const char
        *configId("configid"),
        *help("help");

    namespace po = boost::program_options;
    po::options_description description;
    description.add_options()
        (configId, po::value<std::string > (), "id to request config for")
        (help, "help");

    try {
        po::variables_map values;
        po::store(
                po::parse_command_line(argc, argv, description),
                values);

        if (exists(help, values)) {
            std::cout <<description;
            return 0;
        }
        ensureExists(configId, values);

        FileDistributorApplication application(
                values[configId].as<std::string > ());
        return application.Entry(argc, argv);

    } catch(ProgramOptionException& e) {
        std::cerr <<e._msg <<std::endl;
        return -1;
    }
}

namespace {

class InitSignals {
public:
    InitSignals() { initSignals(); }
};

InitSignals _G_initSignals __attribute__ ((init_priority (101)));

}

int
main(int argc, char** argv) {
    if (askedToShutDown()) { return 0; }
    EV_STARTED(programName);

    std::srand(std::time(0));
    filedistribution::ZKLogging loggingGuard;

    return executeApplication(argc, argv);
}
<commit_msg>c++11 lambdas<commit_after>// Copyright 2016 Yahoo Inc. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include <vespa/fastos/fastos.h>
#include <iostream>
#include <string>
#include <cstdlib>

#include <boost/program_options.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/scope_exit.hpp>

#include <vespa/fastos/app.h>
#include <vespa/config-zookeepers.h>

#include <vespa/fileacquirer/config-filedistributorrpc.h>
#include <vespa/filedistribution/distributor/config-filedistributor.h>
#include <vespa/filedistribution/model/config-filereferences.h>

#include <vespa/filedistribution/distributor/filedistributortrackerimpl.h>
#include <vespa/filedistribution/distributor/filedownloadermanager.h>
#include <vespa/filedistribution/distributor/filedownloader.h>
#include <vespa/filedistribution/distributor/signalhandling.h>
#include <vespa/filedistribution/distributor/state_server_impl.h>

#include <vespa/filedistribution/model/filedistributionmodelimpl.h>
#include <vespa/filedistribution/rpc/filedistributorrpc.h>
#include <vespa/filedistribution/common/exception.h>
#include <vespa/filedistribution/common/componentsdeleter.h>

namespace {
const char* programName = "filedistributor";
}

#include <vespa/log/log.h>
LOG_SETUP(programName);

using namespace std::literals;

using namespace filedistribution;
using cloud::config::ZookeepersConfig;
using cloud::config::filedistribution::FiledistributorConfig;
using cloud::config::filedistribution::FiledistributorrpcConfig;

class FileDistributor : public config::IFetcherCallback<ZookeepersConfig>,
                        public config::IFetcherCallback<FiledistributorConfig>,
                        public config::IFetcherCallback<FiledistributorrpcConfig>,
                        public config::IGenerationCallback
{
    class Components {
        ComponentsDeleter _componentsDeleter;
    public:
        const std::shared_ptr<ZKFacade> _zk;
        const std::shared_ptr<FileDistributionModelImpl> _model;
        const std::shared_ptr<FileDistributorTrackerImpl> _tracker;
        const std::shared_ptr<FileDownloader> _downloader;
        const FileDownloaderManager::SP _manager;
        const FileDistributorRPC::SP _rpcHandler;
        const std::shared_ptr<StateServerImpl> _stateServer;

    private:
        std::thread _downloaderEventLoopThread;
        config::ConfigFetcher _configFetcher;

        template <class T>
        typename std::shared_ptr<T> track(T* component) {
            return _componentsDeleter.track(component);
        }

    public:
        Components(const Components &) = delete;
        Components & operator = (const Components &) = delete;

        Components(const std::shared_ptr<ExceptionRethrower>& exceptionRethrower,
                   const config::ConfigUri & configUri,
                   const ZookeepersConfig& zooKeepersConfig,
                   const FiledistributorConfig& fileDistributorConfig,
                   const FiledistributorrpcConfig& rpcConfig)
            :_zk(track(new ZKFacade(zooKeepersConfig.zookeeperserverlist, exceptionRethrower))),
             _model(track(new FileDistributionModelImpl(
                                     fileDistributorConfig.hostname,
                                     fileDistributorConfig.torrentport,
                                     _zk,
                                     exceptionRethrower))),
             _tracker(track(new FileDistributorTrackerImpl(_model))),
             _downloader(track(new FileDownloader(_tracker,
                                                  fileDistributorConfig.hostname,
                                                  fileDistributorConfig.torrentport,
                                                  boost::filesystem::path(fileDistributorConfig.filedbpath)))),
             _manager(track(new FileDownloaderManager(_downloader, _model))),
             _rpcHandler(track(new FileDistributorRPC(rpcConfig.connectionspec, _manager))),
             _stateServer(track(new StateServerImpl(fileDistributorConfig.stateport))),
             _downloaderEventLoopThread([&] () { _downloader->runEventLoop(); }),
             _configFetcher(configUri.getContext())

        {
            _manager->start();
            _rpcHandler->start();

            _tracker->setDownloader(_downloader);
            _configFetcher.subscribe<FilereferencesConfig>(configUri.getConfigId(), _model.get());
            _configFetcher.start();
            updatedConfig(_configFetcher.getGeneration());
        }

        void updatedConfig(int64_t generation) {
            vespalib::ComponentConfigProducer::Config curr("filedistributor", generation);
            _stateServer->myComponents.addConfig(curr);
        }

        ~Components() {
            _configFetcher.close();
            //Do not waste time retrying zookeeper operations when going down.
            _zk->disableRetries();

            _downloader->close();
            _downloaderEventLoopThread.join();
        }

    };

    typedef std::lock_guard<std::mutex> LockGuard;
    std::mutex _configMutex;

    bool _completeReconfigurationNeeded;
    std::unique_ptr<ZookeepersConfig> _zooKeepersConfig;
    std::unique_ptr<FiledistributorConfig> _fileDistributorConfig;
    std::unique_ptr<FiledistributorrpcConfig> _rpcConfig;

    std::shared_ptr<ExceptionRethrower> _exceptionRethrower;
    std::unique_ptr<Components> _components;
public:
    FileDistributor(const FileDistributor &) = delete;
    FileDistributor & operator = (const FileDistributor &) = delete;
    FileDistributor()
        : _configMutex(),
          _completeReconfigurationNeeded(false),
          _zooKeepersConfig(),
          _fileDistributorConfig(),
          _rpcConfig(),
          _exceptionRethrower(),
          _components()
    { }

    void notifyGenerationChange(int64_t generation) {
        if (_components && ! completeReconfigurationNeeded()) {
            _components->updatedConfig(generation);
        }
    }

    //configure overrides
    void configure(std::unique_ptr<ZookeepersConfig> config) {
        LockGuard guard(_configMutex);
        _zooKeepersConfig = std::move(config);
        _completeReconfigurationNeeded = true;
    }

    void configure(std::unique_ptr<FiledistributorConfig> config) {
        LockGuard guard(_configMutex);
        if (_fileDistributorConfig.get() != NULL &&
            (config->torrentport != _fileDistributorConfig->torrentport ||
             config->stateport   != _fileDistributorConfig->stateport ||
             config->hostname    != _fileDistributorConfig->hostname ||
             config->filedbpath  != _fileDistributorConfig->filedbpath))
        {
            _completeReconfigurationNeeded = true;
        } else if (_components.get()) {
            configureSpeedLimits(*config);
        }
        _fileDistributorConfig = std::move(config);

    }

    void configure(std::unique_ptr<FiledistributorrpcConfig> config) {
        LockGuard guard(_configMutex);
        _rpcConfig = std::move(config);
        _completeReconfigurationNeeded = true;
    }

    void run(const config::ConfigUri & configUri) {
        while (!askedToShutDown()) {
            clearReinitializeFlag();
            _exceptionRethrower.reset(new ExceptionRethrower());
            runImpl(configUri);

            if (_exceptionRethrower->exceptionStored())
                _exceptionRethrower->rethrow();
        }
    }

    static void ensureExceptionsStored(const std::shared_ptr<ExceptionRethrower>& exceptionRethrower) {
        //TODO: this is somewhat hackish, refactor to eliminate this later.
        LOG(debug, "Waiting for shutdown");
        for (int i=0; i<50 && !exceptionRethrower.unique(); ++i) {
            std::this_thread::sleep_for(100ms);
        }
        LOG(debug, "Done waiting for shutdown");

        if (!exceptionRethrower.unique()) {
            EV_STOPPING(programName, "Forced termination");
            kill(getpid(), SIGKILL);
        }
    }

    void createComponents(const std::shared_ptr<ExceptionRethrower>& exceptionRethrower, const config::ConfigUri & configUri) {
        LockGuard guard(_configMutex);
        _components.reset(
                new Components(exceptionRethrower,
                        configUri,
                        *_zooKeepersConfig,
                        *_fileDistributorConfig,
                        *_rpcConfig));

        configureSpeedLimits(*_fileDistributorConfig);
        _completeReconfigurationNeeded = false;
    }

    bool completeReconfigurationNeeded() {
        LockGuard guard(_configMutex);
        if (_completeReconfigurationNeeded) {
            LOG(debug, "Complete reconfiguration needed");
        }
        return _completeReconfigurationNeeded;
    }

    void configureSpeedLimits(const FiledistributorConfig& config) {
        FileDownloader& downloader = *_components->_downloader;
        downloader.setMaxDownloadSpeed(config.maxdownloadspeed);
        downloader.setMaxUploadSpeed(config.maxuploadspeed);
    }

    //avoid warning due to scope exit macro
#pragma GCC diagnostic ignored "-Wshadow"
    void runImpl(const config::ConfigUri & configUri) {

        BOOST_SCOPE_EXIT((&_components)(&_exceptionRethrower)) {
            _components.reset();
            //Ensures that any exception stored during destruction will be available when returning.
            ensureExceptionsStored(_exceptionRethrower);
        } BOOST_SCOPE_EXIT_END

        createComponents(_exceptionRethrower, configUri);

        // We do not want back to back reinitializing as it gives zero time for serving
        // some torrents.
        int postPoneAskedToReinitializedSecs = 50;

        while (!askedToShutDown() &&
	       (postPoneAskedToReinitializedSecs > 0 || !askedToReinitialize()) &&
	       !completeReconfigurationNeeded() &&
	       !_exceptionRethrower->exceptionStored()) {
	  postPoneAskedToReinitializedSecs--;
	  std::this_thread::sleep_for(1s);
        }
    }
};

//TODO: use pop in gcc 4.6
#pragma GCC diagnostic warning "-Wshadow"

class FileDistributorApplication : public FastOS_Application {
    const config::ConfigUri _configUri;
public:
    FileDistributorApplication(const config::ConfigUri & configUri);

    //overrides
    int Main();
};

namespace {

struct ProgramOptionException {
    std::string _msg;
    ProgramOptionException(const std::string & msg)
        : _msg(msg)
    {}
};

bool exists(const std::string& optionName, const boost::program_options::variables_map& map) {
    return map.find(optionName) != map.end();
}

void ensureExists(const std::string& optionName, const boost::program_options::variables_map& map \
                  ) {
    if (!exists(optionName, map)) {
        throw ProgramOptionException("Error: Missing option " + optionName);
    }
}

} //anonymous namespace

FileDistributorApplication::FileDistributorApplication(const config::ConfigUri & configUri)
    :_configUri(configUri) {
}

int
FileDistributorApplication::Main() {
    try {
        FileDistributor distributor;

        config::ConfigFetcher configFetcher(_configUri.getContext());
        configFetcher.subscribe<ZookeepersConfig>(_configUri.getConfigId(), &distributor);
        configFetcher.subscribe<FiledistributorConfig>(_configUri.getConfigId(), &distributor);
        configFetcher.subscribe<FiledistributorrpcConfig>(_configUri.getConfigId(), &distributor);
        configFetcher.subscribeGenerationChanges(&distributor);
        configFetcher.start();

        distributor.run(_configUri);

        EV_STOPPING(programName, "Clean exit");
        return 0;
    } catch(const FileDoesNotExistException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 1;
    } catch(const ZKNodeDoesNotExistsException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 2;
    } catch(const ZKSessionExpired & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 3;
    } catch(const config::ConfigTimeoutException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 4;
    } catch(const FailedListeningException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 5;
    } catch(const ZKGenericException & e) {
        std::string s = boost::diagnostic_information(e);
        EV_STOPPING(programName, s.c_str());
        return 99;
    } catch(const boost::unknown_exception & e) {
        std::string s = boost::diagnostic_information(e);
        LOG(warning, "Caught '%s'", s.c_str());
        EV_STOPPING(programName, s.c_str());
        return 255;
#if 0
    /*
     These are kept hanging around for reference as to how it was when we just held our ears
     singing "na, na, na, na..." no matter if the sun was shining or if the world imploded.
    */
    } catch(const boost::exception& e) {
        std::string s = boost::diagnostic_information(e);
        LOG(error, "Caught '%s'", s.c_str());
        EV_STOPPING(programName, s.c_str());
        return -1;
    } catch(const std::string& msg) {
        std::string s = "Error: " + msg;
        LOG(error, "Caught '%s'", s.c_str());
        EV_STOPPING(programName, s.c_str());
        return -1;
#endif
    }
}

int
executeApplication(int argc, char** argv) {
    const char
        *configId("configid"),
        *help("help");

    namespace po = boost::program_options;
    po::options_description description;
    description.add_options()
        (configId, po::value<std::string > (), "id to request config for")
        (help, "help");

    try {
        po::variables_map values;
        po::store(
                po::parse_command_line(argc, argv, description),
                values);

        if (exists(help, values)) {
            std::cout <<description;
            return 0;
        }
        ensureExists(configId, values);

        FileDistributorApplication application(
                values[configId].as<std::string > ());
        return application.Entry(argc, argv);

    } catch(ProgramOptionException& e) {
        std::cerr <<e._msg <<std::endl;
        return -1;
    }
}

namespace {

class InitSignals {
public:
    InitSignals() { initSignals(); }
};

InitSignals _G_initSignals __attribute__ ((init_priority (101)));

}

int
main(int argc, char** argv) {
    if (askedToShutDown()) { return 0; }
    EV_STARTED(programName);

    std::srand(std::time(0));
    filedistribution::ZKLogging loggingGuard;

    return executeApplication(argc, argv);
}
<|endoftext|>


<commit_before>/**
@copyright MIT license; see @ref index or the accompanying LICENSE file.
*/

#include <Onsang/utility.hpp>
#include <Onsang/UI/Defs.hpp>
#include <Onsang/UI/BasicGrid.hpp>

#include <duct/debug.hpp>

namespace Onsang {
namespace UI {

void
BasicGrid::reflow_impl(
	Rect const& area,
	bool const cache
) noexcept {
	base::reflow_impl(area, cache);
	Rect view_frame = get_geometry().get_frame();
	// ++view_frame.pos.x;
	// ++view_frame.pos.y;
	// view_frame.size.width -= 2;
	// view_frame.size.height -= 2;
	reflow_view(view_frame);
	adjust_view();
	queue_header_render();
	queue_cell_render(0, get_row_count());
}

bool
BasicGrid::handle_event_impl(
	UI::Event const& event
) noexcept {
	switch (event.type) {
	case UI::EventType::key_input:
		if (!has_input_control()) {
			bool handled = true;
			switch (event.key_input.code) {
			case KeyCode::up   : row_step(-1); break;
			case KeyCode::down : row_step(+1); break;
			case KeyCode::left : col_step(-1); break;
			case KeyCode::right: col_step(+1); break;
			case KeyCode::home: row_abs(0); break;
			case KeyCode::end : row_abs(max_ce(0, get_row_count() - 1)); break;
			case KeyCode::pgup:
				row_step(min_ce(0, -get_view().fit_count - 1)); break;
			case KeyCode::pgdn:
				row_step(max_ce(0, +get_view().fit_count - 1)); break;

			default:
				switch (event.key_input.cp) {
				case 'k': row_step(-1); break;
				case 'j': row_step(+1); break;
				case 'h': col_step(-1); break;
				case 'l': col_step(+1); break;
				case 'a': select_all(); break;
				case 'A': select_none(); break;
				case ' ': select_toggle(m_cursor.row, 1); break;
				//case 'e': erase(m_cursor.row, 1); break;
				//case 'E': erase_selected(); break;
				//case 'i': insert_after (m_cursor.row, 1); break;
				//case 'I': insert_before(m_cursor.row, 1); break;
				default:
					handled = false;
					break;
				}
			}
			return handled;
		}
		break;

	case UI::EventType::focus_changed:
		queue_cell_render(
			m_cursor.row, m_cursor.row + 1,
			m_cursor.col, m_cursor.col + 1
		);
		return false;

	default:
		break;
	}
	return false;
}

void
BasicGrid::render_impl(
	UI::Widget::RenderData& rd
) noexcept {
	UI::GridRenderData grid_rd{
		rd,
		is_focused(),
		is_focused()
	};
	render_view(
		grid_rd,
		!enum_bitand(
			get_queued_actions(),
			UI::UpdateActions::flag_noclear
		)
	);

	auto const& view = get_view();
	Rect const empty_frame{
		{view.content_frame.pos.x, view.content_frame.pos.y + view.row_count},
		{
			view.content_frame.size.width,
			max_ce(0, view.fit_count - view.row_count)
		}
	};
	if (0 < empty_frame.size.height) {
		grid_rd.rd.terminal.clear_back(empty_frame);
		grid_rd.rd.terminal.put_line(
			empty_frame.pos,
			empty_frame.size.height,
			Axis::vertical,
			tty::make_cell('~',
				tty::Color::blue | tty::Attr::bold,
				tty::Color::term_default
			)
		);
	}
}

void
BasicGrid::content_action(
	UI::ProtoGrid::ContentAction action,
	UI::index_type row_begin,
	UI::index_type count
) noexcept {
	using ContentAction = UI::ProtoGrid::ContentAction;

	DUCT_ASSERTE(get_row_count() == static_cast<signed>(m_rows.size()));
	// Cast insert_after in terms of insert_before
	if (ContentAction::insert_after == action) {
		++row_begin;
	}
	row_begin = value_clamp(row_begin, 0, get_row_count());
	auto const row_end = min_ce(row_begin + count, get_row_count());
	auto clear_flag = UI::UpdateActions::none;
	switch (action) {
	// Select
	case ContentAction::select:
	case ContentAction::unselect: {
		bool const enable = ContentAction::select == action;
		auto const end = m_rows.begin() + row_end;
		for (auto it = m_rows.begin() + row_begin; end > it; ++it) {
			it->states.set(
				Row::Flags::selected,
				enable
			);
		}
		queue_cell_render(row_begin, row_end);
		clear_flag = UI::UpdateActions::flag_noclear;
	}	break;

	case ContentAction::select_toggle: {
		auto const end = m_rows.begin() + row_end;
		for (auto it = m_rows.begin() + row_begin; end > it; ++it) {
			it->states.set(
				Row::Flags::selected,
				!it->states.test(Row::Flags::selected)
			);
		}
		queue_cell_render(row_begin, row_end);
		clear_flag = UI::UpdateActions::flag_noclear;
	}	break;

	// Insert
	case ContentAction::insert_after: // fall-through
	case ContentAction::insert_before:
		m_rows.insert(
			m_rows.begin() + row_begin,
			static_cast<std::size_t>(count),
			Row{}
		);
		content_action_internal(
			ContentAction::insert_before,
			row_begin,
			count
		);
		if (1 == get_row_count()) {
			set_cursor(m_cursor.col, 0);
		} else if (row_begin <= m_cursor.row) {
			set_cursor(m_cursor.col, m_cursor.row + count);
		}
		adjust_view();
		break;

	// Erase
	case ContentAction::erase:
		if (content_erase(row_begin, count)) {
			m_rows.erase(
				m_rows.cbegin() + row_begin,
				m_rows.cbegin() + row_end
			);
			content_action_internal(
				ContentAction::erase,
				row_begin,
				count
			);
		} else {
			return;
		}
		break;

	case ContentAction::erase_selected:
		UI::index_type
			head = 0,
			tail = head,
			rcount = 0
		;
		while (get_row_count() >= tail) {
			if (
				get_row_count() == tail ||
				!m_rows[tail].states.test(Row::Flags::selected)
			) {
				if (tail > head) {
					rcount = tail - head;
					if (content_erase(head, rcount)) {
						m_rows.erase(
							m_rows.cbegin() + head,
							m_rows.cbegin() + tail
						);
						content_action_internal(ContentAction::erase, head, rcount);
						/*if (head <= m_cursor.row) {
							set_cursor(
								m_cursor.col,
								max_ce(head, m_cursor.row - rcount)
							);
						}*/
						// tail is not selected, so no sense in checking it again
						tail = ++head;
					} else {
						head = ++tail;
					}
				} else {
					head = ++tail;
				}
			} else {
				++tail;
			}
		}
		break;
	} // switch (action)

	// Post action
	switch (action) {
	case ContentAction::erase:
	case ContentAction::erase_selected:
		// Let cursor clamp to new bounds
		set_cursor(m_cursor.col, m_cursor.row);
		adjust_view();
		break;

	default:
		break;
	}

	queue_actions(enum_combine(
		UI::UpdateActions::render,
		clear_flag
	));
}

void
BasicGrid::adjust_view() noexcept {
	auto const& view = get_view();
	if (
		view.row_range.x >  m_cursor.row ||
		view.row_range.y <= m_cursor.row ||
		view.col_range.x >  m_cursor.col ||
		view.col_range.y <= m_cursor.col
	) {
		auto row_begin = m_cursor.row;
		if (view.row_range.y <= m_cursor.row) {
			row_begin -= view.fit_count - 1;
		}
		update_view(
			row_begin,
			row_begin + view.fit_count,
			0,
			get_col_count(),
			true
		);
		queue_actions(UI::UpdateActions::render);
	}
}

void
BasicGrid::set_cursor(
	UI::index_type col,
	UI::index_type row
) noexcept {
	col = value_clamp(col, 0, max_ce(0, get_col_count() - 1));
	row = value_clamp(row, 0, max_ce(0, get_row_count() - 1));
	if (col != m_cursor.col || row != m_cursor.row) {
		if (
			value_in_bounds(m_cursor.row, 0, get_row_count()) &&
			value_in_bounds(m_cursor.col, 0, get_col_count())
		) {
			queue_cell_render(
				m_cursor.row, m_cursor.row + 1,
				m_cursor.col, m_cursor.col + 1
			);
		}
		if (!m_rows.empty()) {
			queue_cell_render(
				row, row + 1,
				col, col + 1
			);
		}
		queue_actions(enum_combine(
			UI::UpdateActions::render,
			UI::UpdateActions::flag_noclear
		));
		m_cursor.col = col;
		m_cursor.row = row;
		adjust_view();
	}
}

} // namespace UI
} // namespace Onsang
<commit_msg>UI::BasicGrid: set CSL location during focus change and cursor change.<commit_after>/**
@copyright MIT license; see @ref index or the accompanying LICENSE file.
*/

#include <Onsang/utility.hpp>
#include <Onsang/UI/Defs.hpp>
#include <Onsang/UI/BasicGrid.hpp>
#include <Onsang/App.hpp>

#include <duct/debug.hpp>

namespace Onsang {
namespace UI {

void
BasicGrid::reflow_impl(
	Rect const& area,
	bool const cache
) noexcept {
	base::reflow_impl(area, cache);
	Rect view_frame = get_geometry().get_frame();
	// ++view_frame.pos.x;
	// ++view_frame.pos.y;
	// view_frame.size.width -= 2;
	// view_frame.size.height -= 2;
	reflow_view(view_frame);
	adjust_view();
	queue_header_render();
	queue_cell_render(0, get_row_count());
}

bool
BasicGrid::handle_event_impl(
	UI::Event const& event
) noexcept {
	switch (event.type) {
	case UI::EventType::key_input:
		if (!has_input_control()) {
			bool handled = true;
			switch (event.key_input.code) {
			case KeyCode::up   : row_step(-1); break;
			case KeyCode::down : row_step(+1); break;
			case KeyCode::left : col_step(-1); break;
			case KeyCode::right: col_step(+1); break;
			case KeyCode::home: row_abs(0); break;
			case KeyCode::end : row_abs(max_ce(0, get_row_count() - 1)); break;
			case KeyCode::pgup:
				row_step(min_ce(0, -get_view().fit_count - 1)); break;
			case KeyCode::pgdn:
				row_step(max_ce(0, +get_view().fit_count - 1)); break;

			default:
				switch (event.key_input.cp) {
				case 'k': row_step(-1); break;
				case 'j': row_step(+1); break;
				case 'h': col_step(-1); break;
				case 'l': col_step(+1); break;
				case 'a': select_all(); break;
				case 'A': select_none(); break;
				case ' ': select_toggle(m_cursor.row, 1); break;
				//case 'e': erase(m_cursor.row, 1); break;
				//case 'E': erase_selected(); break;
				//case 'i': insert_after (m_cursor.row, 1); break;
				//case 'I': insert_before(m_cursor.row, 1); break;
				default:
					handled = false;
					break;
				}
			}
			return handled;
		}
		break;

	case UI::EventType::focus_changed:
		queue_cell_render(
			m_cursor.row, m_cursor.row + 1,
			m_cursor.col, m_cursor.col + 1
		);
		if (is_focused()) {
			App::instance.m_ui.csline->set_location(
				std::to_string(m_cursor.row) + ", " +
				std::to_string(m_cursor.col)
			);
		} else {
			App::instance.m_ui.csline->clear_location();
		}
		return false;

	default:
		break;
	}
	return false;
}

void
BasicGrid::render_impl(
	UI::Widget::RenderData& rd
) noexcept {
	UI::GridRenderData grid_rd{
		rd,
		is_focused(),
		is_focused()
	};
	render_view(
		grid_rd,
		!enum_bitand(
			get_queued_actions(),
			UI::UpdateActions::flag_noclear
		)
	);

	auto const& view = get_view();
	Rect const empty_frame{
		{view.content_frame.pos.x, view.content_frame.pos.y + view.row_count},
		{
			view.content_frame.size.width,
			max_ce(0, view.fit_count - view.row_count)
		}
	};
	if (0 < empty_frame.size.height) {
		grid_rd.rd.terminal.clear_back(empty_frame);
		grid_rd.rd.terminal.put_line(
			empty_frame.pos,
			empty_frame.size.height,
			Axis::vertical,
			tty::make_cell('~',
				tty::Color::blue | tty::Attr::bold,
				tty::Color::term_default
			)
		);
	}
}

void
BasicGrid::content_action(
	UI::ProtoGrid::ContentAction action,
	UI::index_type row_begin,
	UI::index_type count
) noexcept {
	using ContentAction = UI::ProtoGrid::ContentAction;

	DUCT_ASSERTE(get_row_count() == static_cast<signed>(m_rows.size()));
	// Cast insert_after in terms of insert_before
	if (ContentAction::insert_after == action) {
		++row_begin;
	}
	row_begin = value_clamp(row_begin, 0, get_row_count());
	auto const row_end = min_ce(row_begin + count, get_row_count());
	auto clear_flag = UI::UpdateActions::none;
	switch (action) {
	// Select
	case ContentAction::select:
	case ContentAction::unselect: {
		bool const enable = ContentAction::select == action;
		auto const end = m_rows.begin() + row_end;
		for (auto it = m_rows.begin() + row_begin; end > it; ++it) {
			it->states.set(
				Row::Flags::selected,
				enable
			);
		}
		queue_cell_render(row_begin, row_end);
		clear_flag = UI::UpdateActions::flag_noclear;
	}	break;

	case ContentAction::select_toggle: {
		auto const end = m_rows.begin() + row_end;
		for (auto it = m_rows.begin() + row_begin; end > it; ++it) {
			it->states.set(
				Row::Flags::selected,
				!it->states.test(Row::Flags::selected)
			);
		}
		queue_cell_render(row_begin, row_end);
		clear_flag = UI::UpdateActions::flag_noclear;
	}	break;

	// Insert
	case ContentAction::insert_after: // fall-through
	case ContentAction::insert_before:
		m_rows.insert(
			m_rows.begin() + row_begin,
			static_cast<std::size_t>(count),
			Row{}
		);
		content_action_internal(
			ContentAction::insert_before,
			row_begin,
			count
		);
		if (1 == get_row_count()) {
			set_cursor(m_cursor.col, 0);
		} else if (row_begin <= m_cursor.row) {
			set_cursor(m_cursor.col, m_cursor.row + count);
		}
		adjust_view();
		break;

	// Erase
	case ContentAction::erase:
		if (content_erase(row_begin, count)) {
			m_rows.erase(
				m_rows.cbegin() + row_begin,
				m_rows.cbegin() + row_end
			);
			content_action_internal(
				ContentAction::erase,
				row_begin,
				count
			);
		} else {
			return;
		}
		break;

	case ContentAction::erase_selected:
		UI::index_type
			head = 0,
			tail = head,
			rcount = 0
		;
		while (get_row_count() >= tail) {
			if (
				get_row_count() == tail ||
				!m_rows[tail].states.test(Row::Flags::selected)
			) {
				if (tail > head) {
					rcount = tail - head;
					if (content_erase(head, rcount)) {
						m_rows.erase(
							m_rows.cbegin() + head,
							m_rows.cbegin() + tail
						);
						content_action_internal(ContentAction::erase, head, rcount);
						/*if (head <= m_cursor.row) {
							set_cursor(
								m_cursor.col,
								max_ce(head, m_cursor.row - rcount)
							);
						}*/
						// tail is not selected, so no sense in checking it again
						tail = ++head;
					} else {
						head = ++tail;
					}
				} else {
					head = ++tail;
				}
			} else {
				++tail;
			}
		}
		break;
	} // switch (action)

	// Post action
	switch (action) {
	case ContentAction::erase:
	case ContentAction::erase_selected:
		// Let cursor clamp to new bounds
		set_cursor(m_cursor.col, m_cursor.row);
		adjust_view();
		break;

	default:
		break;
	}

	queue_actions(enum_combine(
		UI::UpdateActions::render,
		clear_flag
	));
}

void
BasicGrid::adjust_view() noexcept {
	auto const& view = get_view();
	if (
		view.row_range.x >  m_cursor.row ||
		view.row_range.y <= m_cursor.row ||
		view.col_range.x >  m_cursor.col ||
		view.col_range.y <= m_cursor.col
	) {
		auto row_begin = m_cursor.row;
		if (view.row_range.y <= m_cursor.row) {
			row_begin -= view.fit_count - 1;
		}
		update_view(
			row_begin,
			row_begin + view.fit_count,
			0,
			get_col_count(),
			true
		);
		queue_actions(UI::UpdateActions::render);
	}
}

void
BasicGrid::set_cursor(
	UI::index_type col,
	UI::index_type row
) noexcept {
	col = value_clamp(col, 0, max_ce(0, get_col_count() - 1));
	row = value_clamp(row, 0, max_ce(0, get_row_count() - 1));
	if (col != m_cursor.col || row != m_cursor.row) {
		if (
			value_in_bounds(m_cursor.row, 0, get_row_count()) &&
			value_in_bounds(m_cursor.col, 0, get_col_count())
		) {
			queue_cell_render(
				m_cursor.row, m_cursor.row + 1,
				m_cursor.col, m_cursor.col + 1
			);
		}
		if (!m_rows.empty()) {
			queue_cell_render(
				row, row + 1,
				col, col + 1
			);
		}
		queue_actions(enum_combine(
			UI::UpdateActions::render,
			UI::UpdateActions::flag_noclear
		));
		m_cursor.col = col;
		m_cursor.row = row;
		adjust_view();
		if (is_focused()) {
			App::instance.m_ui.csline->set_location(
				std::to_string(m_cursor.row) + ", " +
				std::to_string(m_cursor.col)
			);
		}
	}
}

} // namespace UI
} // namespace Onsang
<|endoftext|>


<commit_before>#include <assert.h>
#include <array>

#include <boost/utility/string_ref.hpp>
#include "restc-cpp/restc-cpp.h"
#include "restc-cpp/RequestBody.h"
#include "restc-cpp/DataWriter.h"

using namespace std;

namespace restc_cpp {
namespace impl {

// TODO: Change to memory mapped IO

class RequestBodyFileImpl : public RequestBody
{
public:
    RequestBodyFileImpl(boost::filesystem::path path)
    : path_{move(path)}
    , size_{boost::filesystem::file_size(path_)}
    {
        file_ = make_unique<ifstream>(path_.string(), ios::binary);
    }

    Type GetType() const noexcept override {
        return Type::FIXED_SIZE;
    }

    uint64_t GetFixedSize() const override {
        return size_;
    }

    bool GetData(write_buffers_t & buffers) override {
        const auto bytes_left = size_ - bytes_read_;
        if (bytes_left == 0) {
            eof_ = true;
            file_.reset();
            return false;
        }

        auto want_bytes = min(buffer_.size(), static_cast<size_t>(bytes_left));
        file_->read(buffer_.data(), want_bytes);
        const size_t read_this_time = static_cast<size_t>(file_->gcount());
        if (read_this_time == 0) {
            const auto err = errno;
            throw new IoException(string{"file read failed: "}
                + to_string(err) + " " + strerror(err));
        }

        bytes_read_ += read_this_time;
        buffers.push_back({buffer_.data(), read_this_time});
        return true;
    }

    void Reset() override {
        eof_ = false;
        bytes_read_ = 0;
        file_->clear();
        file_->seekg(0);
    }

private:
    bool eof_ = false;
    boost::filesystem::path path_;
    unique_ptr<ifstream> file_;
    uint64_t bytes_read_ = 0;
    const uint64_t size_;
    array<char, 1024 * 8> buffer_;
};

} // impl

unique_ptr<RequestBody> RequestBody::CreateFileBody(
    boost::filesystem::path path) {

    return make_unique<impl::RequestBodyFileImpl>(move(path));
}

} // restc_cpp

<commit_msg>Added logging<commit_after>#include <assert.h>
#include <array>

#include <boost/utility/string_ref.hpp>
#include "restc-cpp/restc-cpp.h"
#include "restc-cpp/logging.h"
#include "restc-cpp/RequestBody.h"
#include "restc-cpp/DataWriter.h"

using namespace std;

namespace restc_cpp {
namespace impl {

// TODO: Change to memory mapped IO

class RequestBodyFileImpl : public RequestBody
{
public:
    RequestBodyFileImpl(boost::filesystem::path path)
    : path_{move(path)}
    , size_{boost::filesystem::file_size(path_)}
    {
        file_ = make_unique<ifstream>(path_.string(), ios::binary);
    }

    Type GetType() const noexcept override {
        return Type::FIXED_SIZE;
    }

    uint64_t GetFixedSize() const override {
        return size_;
    }

    bool GetData(write_buffers_t & buffers) override {
        const auto bytes_left = size_ - bytes_read_;
        if (bytes_left == 0) {
            eof_ = true;
            file_.reset();
            RESTC_CPP_LOG_DEBUG << "Successfully uploaded file "
                << path_
                << " of size " << size_ << " bytes.";
            return false;
        }

        auto want_bytes = min(buffer_.size(), static_cast<size_t>(bytes_left));
        file_->read(buffer_.data(), want_bytes);
        const size_t read_this_time = static_cast<size_t>(file_->gcount());
        if (read_this_time == 0) {
            const auto err = errno;
            throw new IoException(string{"file read failed: "}
                + to_string(err) + " " + strerror(err));
        }

        bytes_read_ += read_this_time;
        buffers.push_back({buffer_.data(), read_this_time});
        return true;
    }

    void Reset() override {
        eof_ = false;
        bytes_read_ = 0;
        file_->clear();
        file_->seekg(0);
    }

private:
    bool eof_ = false;
    boost::filesystem::path path_;
    unique_ptr<ifstream> file_;
    uint64_t bytes_read_ = 0;
    const uint64_t size_;
    array<char, 1024 * 8> buffer_;
};

} // impl

unique_ptr<RequestBody> RequestBody::CreateFileBody(
    boost::filesystem::path path) {

    return make_unique<impl::RequestBodyFileImpl>(move(path));
}

} // restc_cpp

<|endoftext|>


<commit_before>#include "controller_node.h"
#include <std_msgs/Bool.h>
#include <std_srvs/Empty.h>
#include <iostream>

#define STICK_DEADZONE 0.1f

// TODO delete
// eval
#include <fstream>
static std::ofstream filestream_;

Controller::Controller() :
    goal_reached_(true),
    search_for_plane_(false),
    stick_to_plane_(false),
    sticking_distance_(0.5f),
    correction_speed_(0.5f),
    is_active_(false)
{
    std::cout << "Controller node started..." << std::endl;

    nh_ = ros::NodeHandle("");
    private_nh_ = ros::NodeHandle("~");

    // set stick deadzone
    stick_deadzone_ = Deadzone<float>(STICK_DEADZONE);

    // get Ros parameters and set variables
    private_nh_.param("speedForward", speedX_, 0.5f);
    private_nh_.param("speedRight", speedY_, 0.5f);
    private_nh_.param("speedUp", speedZ_, 0.5f);
    private_nh_.param("speedYaw", speedYaw_, 0.05f);

    // set subscriber and publisher
    sub_joy_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Controller::callback, this);
    sub_mocap_pose_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("estimated_transform", 10, &Controller::setMocapPose, this);
    sub_plane_tf_ = nh_.subscribe<geometry_msgs::TransformStamped>("plane", 10, &Controller::processPlaneMsg, this);
    pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("command/pose", 1);
    pub_stickToSurface_ = nh_.advertise<std_msgs::Bool>("controller/stickToSurface", 10);

    std::cout << "speeds: " << speedX_ << " " << speedY_ << " " << speedZ_ << " " << speedYaw_ << std::endl;

    // identity rotation matrix as quaternion
    tf::Quaternion q;
    q.setRPY(0,0,0);

    // init mocap_tf
    mavToWorld_.setOrigin( tf::Vector3(0,0,0) );
    mavToWorld_.setRotation(q);

    // init transform
    transform_.setOrigin( tf::Vector3(0,0,0) );
    transform_.setRotation(q);

    // TBD if we should be able to change this via launch file
    // init hover transform
    hover_goal_tf_.setOrigin( tf::Vector3(0,0,1) );
    hover_goal_tf_.setRotation(q);

    // init plane_tf
    plane_tf_.setOrigin(tf::Vector3(2,0,0));
    tf::Quaternion plane_q;
    plane_q.setRPY(0,0,-M_PI/4.f);
    plane_tf_.setRotation(plane_q);

    // init snap_goal_tf
    snap_goal_tf_.setOrigin(tf::Vector3(0,0,0));
    snap_goal_tf_.setRotation(q);

    // hardcoded sensorToMav transform
    tf::Transform mavToWorld;
    mavToWorld.setOrigin(tf::Vector3(0,0,0.117));
    mavToWorld.setRotation(tf::Quaternion(0,0,0,1));
    tf::Transform sensorToWorld;
    sensorToWorld.setOrigin(tf::Vector3(0.133,0,0.0605));
    sensorToWorld.setRotation(tf::Quaternion(0,0.17365,0,0.98481));
    sensorToMav_ = mavToWorld.inverse() * sensorToWorld;
}

void Controller::setMocapPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
    mavToWorld_.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z) );
    mavToWorld_.setRotation( tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w) );

    /// takeoff and hover blocks until goal is reached
    if(!goal_reached_) {
        tf::Transform diff = mavToWorld_.inverse() * hover_goal_tf_;
        if(diff.getOrigin().length() < 0.1f) {
            goal_reached_ = true;
        } else {
            transform_ = diff;
        }
    }

    if (!is_active_) {
	return;
    }
    is_active_ = false;

    // world transform
    tf::Transform curr_transform;
    /// stick to plane
    if(stick_to_plane_ && goal_reached_) {
        testPlanes();
        tf::Vector3 diff = snap_goal_tf_.getOrigin() - mavToWorld_.getOrigin();
        curr_transform.setOrigin(mavToWorld_.getOrigin() + correction_speed_*diff);
        curr_transform.setRotation(snap_goal_tf_.getRotation());
    } else {
        curr_transform = mavToWorld_ * transform_;
    }

    /// convert tf into pose and publish the pose
    tf::Vector3 desired_pos = curr_transform.getOrigin();
    tf::Quaternion desired_rot = curr_transform.getRotation();

    geometry_msgs::PoseStamped pose;
    // set header
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "world";
    // set pose
    pose.pose.position.x = desired_pos.x();
    pose.pose.position.y = desired_pos.y();
    pose.pose.position.z = desired_pos.z();
    pose.pose.orientation.x = desired_rot.x();
    pose.pose.orientation.y = desired_rot.y();
    pose.pose.orientation.z = desired_rot.z();
    pose.pose.orientation.w = desired_rot.w();
    pub_pose_.publish(pose);

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(curr_transform, ros::Time::now(), "world", "controller") );
}

void Controller::callback(const sensor_msgs::Joy::ConstPtr& joy)
{
    if(std::abs(joy->axes[PS3_AXIS_STICK_RIGHT_UPWARDS]) <= STICK_DEADZONE &&
	std::abs(joy->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]) <= STICK_DEADZONE &&
	std::abs(joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS]) <= STICK_DEADZONE &&
	std::abs(joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS]) <= STICK_DEADZONE) {
	is_active_ = false;
	return;
    }
    is_active_ = true;

    /// translation from controller axis
    float jx = speedX_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_RIGHT_UPWARDS]);
    float jy = speedY_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]);
    float jz = speedZ_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS]);
    // yaw from axis
    float jr = speedYaw_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS]);

    // save only the latest relative transform in global variable transform_
    transform_.setOrigin( tf::Vector3(jx,jy,jz) );
    tf::Quaternion q;
    q.setRPY(0,0,jr);
    transform_.setRotation(q);

    /// buttons
    // listen for take off button pressed
    if(joy->buttons[PS3_BUTTON_PAIRING] || joy->buttons[PS3_BUTTON_START]) {
        goal_reached_ = false;
        takeoffAndHover();
    }
    // tell SurfaceDetection to search for a plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_2]) {
        search_for_plane_ = true;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_2]) {
        search_for_plane_ = false;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    // enable or disable sticking to the plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_1] && !stick_to_plane_) {
        stick_to_plane_ = true;

        // set current distance to surface as sticking distance
        Eigen::Vector3f plane_pos = Eigen::Vector3f( plane_tf_.getOrigin().x(), plane_tf_.getOrigin().y(), plane_tf_.getOrigin().z() );
        tf::Quaternion plane_q = plane_tf_.getRotation();
        Eigen::Quaternionf plane_rot = Eigen::Quaternionf(plane_q.w(),plane_q.x(),plane_q.y(),plane_q.z());
        Eigen::Vector3f normal = plane_rot*forward;
        normal.normalize();
        Eigen::Vector3f curr_pos = Eigen::Vector3f(mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), mavToWorld_.getOrigin().z());
        sticking_distance_ = -normal.dot(curr_pos-plane_pos);

        // TODO delete
        // eval
        filestream_.open("//usr//stud//mueller//eval.txt", std::ofstream::out);
        if(!filestream_) {
            std::cout << "Error: could not open file" << std::endl;
        } else {
            std::cout << "Writing to file..." << std::endl;
        }
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_1] && stick_to_plane_) {
        stick_to_plane_ = false;

        // TODO delete
        // eval
        // ...
        if(filestream_) {
            std::cout << "Closing file..." << std::endl;
            filestream_.close();
        }
    }
    // sticking distance
    if(joy->buttons[PS3_BUTTON_CROSS_UP] && abs(sticking_distance_) > 0.5f) {
        sticking_distance_ -= 0.005f;
    }
    if(joy->buttons[PS3_BUTTON_CROSS_DOWN]) {
        sticking_distance_ += 0.005f;
    }
}

void Controller::takeoffAndHover()
{
    std_srvs::Empty::Request request;
    std_srvs::Empty::Response response;
    if(ros::service::call("euroc2/takeoff", request, response)) {
        tf::Vector3 desired_pos = tf::Vector3( mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), 1.0f);
        tf::Quaternion desired_rot = mavToWorld_.getRotation();

        hover_goal_tf_.setOrigin(desired_pos);
        hover_goal_tf_.setRotation(desired_rot);
    }
}

void Controller::processPlaneMsg(const geometry_msgs::TransformStamped::ConstPtr& msg)
{
    // realtive plane transform
    // planeToCam = plane_tf
    plane_tf_.setOrigin( tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z) );
    plane_tf_.setRotation( tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w) );

    // plane forward is z should be x, conversion of systems
    // camToSensor
    tf::Quaternion q1; q1.setRPY(0,M_PI/2.0,0);
    tf::Quaternion q2; q2.setRPY(-M_PI/2.0,0,0);
    tf::Quaternion opticalToSensor = q2*q1;
    plane_tf_.setOrigin( tf::Matrix3x3(opticalToSensor)*plane_tf_.getOrigin() );
    plane_tf_.setRotation( opticalToSensor*plane_tf_.getRotation() );
    //br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/vi_sensor/ground_truth", "plane_Sensor") );

    // correct the relative camera offset
    // planeToMav = plane_tf
    plane_tf_ = sensorToMav_*plane_tf_;

    // transform into global coordinates
    // planeToWorld = plane_tf_
    plane_tf_ = mavToWorld_*plane_tf_;

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "controller/plane") );
}

void Controller::testPlanes()
{
    //// mav
    // predicted mav tf
    tf::Transform mav_tf = mavToWorld_ * transform_;

    // Eigen conversions
    Eigen::Vector3f mav_pos = Eigen::Vector3f( mav_tf.getOrigin().x(),
                                               mav_tf.getOrigin().y(),
                                               mav_tf.getOrigin().z());

    //// plane
    Eigen::Vector3f plane_pos = Eigen::Vector3f( plane_tf_.getOrigin().x(), plane_tf_.getOrigin().y(), plane_tf_.getOrigin().z() );
    tf::Quaternion plane_q = plane_tf_.getRotation();
    Eigen::Quaternionf plane_rot = Eigen::Quaternionf(plane_q.w(),plane_q.x(),plane_q.y(),plane_q.z());

    //// calculations
    Eigen::Vector3f normal = plane_rot*forward;
    normal.normalize();
    // determine if mav is in front or behind plane normal, take current pos to avoid switching of sides by wrong predictions in mav_tf
    Eigen::Vector3f curr_pos = Eigen::Vector3f(mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), mavToWorld_.getOrigin().z());

    int facing = normal.dot(curr_pos-plane_pos) >= 0 ? 1 : -1;
    // calculate projected position of the mav onto the plane
    Eigen::Vector3f proj_pos = mav_pos + (facing*sticking_distance_ - normal.dot(mav_pos-plane_pos))*normal;

    // set snapping goal tf
    snap_goal_tf_.setOrigin( tf::Vector3(proj_pos.x(), proj_pos.y(), proj_pos.z()) );
    snap_goal_tf_.setRotation(plane_tf_.getRotation());

    // TODO delete
    // eval
    filestream_ << mavToWorld_.getOrigin().x() << "," << mavToWorld_.getOrigin().y() << "," << mavToWorld_.getOrigin().z() << std::endl;
    filestream_ << proj_pos.x() << "," << proj_pos.y() << "," << proj_pos.z() << std::endl;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "te_joy_controller");
    Controller rc;
    ros::spin();
}
<commit_msg>Fixed a contoller bug and enhanced control.<commit_after>#include "controller_node.h"
#include <std_msgs/Bool.h>
#include <std_srvs/Empty.h>
#include <iostream>

#define STICK_DEADZONE 0.1f

// TODO delete
// eval
#include <fstream>
static std::ofstream filestream_;

Controller::Controller() :
    goal_reached_(true),
    search_for_plane_(false),
    stick_to_plane_(false),
    sticking_distance_(0.5f),
    correction_speed_(0.5f),
    is_active_(false)
{
    std::cout << "Controller node started..." << std::endl;

    nh_ = ros::NodeHandle("");
    private_nh_ = ros::NodeHandle("~");

    // set stick deadzone
    stick_deadzone_ = Deadzone<float>(STICK_DEADZONE);

    // get Ros parameters and set variables
    private_nh_.param("speedForward", speedX_, 0.5f);
    private_nh_.param("speedRight", speedY_, 0.5f);
    private_nh_.param("speedUp", speedZ_, 0.5f);
    private_nh_.param("speedYaw", speedYaw_, 0.05f);

    // set subscriber and publisher
    sub_joy_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Controller::callback, this);
    sub_mocap_pose_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("estimated_transform", 10, &Controller::setMocapPose, this);
    sub_plane_tf_ = nh_.subscribe<geometry_msgs::TransformStamped>("plane", 10, &Controller::processPlaneMsg, this);
    pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("command/pose", 1);
    pub_stickToSurface_ = nh_.advertise<std_msgs::Bool>("controller/stickToSurface", 10);

    std::cout << "speeds: " << speedX_ << " " << speedY_ << " " << speedZ_ << " " << speedYaw_ << std::endl;

    // identity rotation matrix as quaternion
    tf::Quaternion q;
    q.setRPY(0,0,0);

    // init mocap_tf
    mavToWorld_.setOrigin( tf::Vector3(0,0,0) );
    mavToWorld_.setRotation(q);

    // init transform
    transform_.setOrigin( tf::Vector3(0,0,0) );
    transform_.setRotation(q);

    // TBD if we should be able to change this via launch file
    // init hover transform
    hover_goal_tf_.setOrigin( tf::Vector3(0,0,1) );
    hover_goal_tf_.setRotation(q);

    // init plane_tf
    plane_tf_.setOrigin(tf::Vector3(2,0,0));
    tf::Quaternion plane_q;
    plane_q.setRPY(0,0,-M_PI/4.f);
    plane_tf_.setRotation(plane_q);

    // init snap_goal_tf
    snap_goal_tf_.setOrigin(tf::Vector3(0,0,0));
    snap_goal_tf_.setRotation(q);

    // hardcoded sensorToMav transform
    tf::Transform mavToWorld;
    mavToWorld.setOrigin(tf::Vector3(0,0,0.117));
    mavToWorld.setRotation(tf::Quaternion(0,0,0,1));
    tf::Transform sensorToWorld;
    sensorToWorld.setOrigin(tf::Vector3(0.133,0,0.0605));
    sensorToWorld.setRotation(tf::Quaternion(0,0.17365,0,0.98481));
    sensorToMav_ = mavToWorld.inverse() * sensorToWorld;
}

void Controller::setMocapPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
    mavToWorld_.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z) );
    mavToWorld_.setRotation( tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w) );

    /// takeoff and hover blocks until goal is reached
    if(!goal_reached_) {
        tf::Transform diff = mavToWorld_.inverse() * hover_goal_tf_;
        if(diff.getOrigin().length() < 0.1f) {
            goal_reached_ = true;
        } else {
            transform_ = diff;
        }
    }

    if (!is_active_) {
	return;
    }
    is_active_ = false;

    // world transform
    tf::Transform curr_transform;
    /// stick to plane
    if(stick_to_plane_ && goal_reached_) {
        testPlanes();
        tf::Vector3 diff = snap_goal_tf_.getOrigin() - mavToWorld_.getOrigin();
        curr_transform.setOrigin(mavToWorld_.getOrigin() + correction_speed_*diff);
	//curr_transform.setOrigin(snap_goal_tf_.getOrigin());
        curr_transform.setRotation(snap_goal_tf_.getRotation());
    } else {
        curr_transform = mavToWorld_ * transform_;
    }

    /// convert tf into pose and publish the pose
    tf::Vector3 desired_pos = curr_transform.getOrigin();
    tf::Quaternion desired_rot = curr_transform.getRotation();

    geometry_msgs::PoseStamped pose;
    // set header
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "world";
    // set pose
    pose.pose.position.x = desired_pos.x();
    pose.pose.position.y = desired_pos.y();
    pose.pose.position.z = desired_pos.z();
    pose.pose.orientation.x = desired_rot.x();
    pose.pose.orientation.y = desired_rot.y();
    pose.pose.orientation.z = desired_rot.z();
    pose.pose.orientation.w = desired_rot.w();
    pub_pose_.publish(pose);

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(curr_transform, ros::Time::now(), "world", "controller") );
}

void Controller::callback(const sensor_msgs::Joy::ConstPtr& joy)
{
    /// buttons
    // listen for take off button pressed
    if(joy->buttons[PS3_BUTTON_PAIRING] || joy->buttons[PS3_BUTTON_START]) {
        goal_reached_ = false;
        takeoffAndHover();
    }
    // tell SurfaceDetection to search for a plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_2]) {
        search_for_plane_ = true;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_2]) {
        search_for_plane_ = false;
        std_msgs::Bool sticking;
        sticking.data = search_for_plane_;
        pub_stickToSurface_.publish(sticking);
    }
    // enable or disable sticking to the plane
    if(joy->buttons[PS3_BUTTON_REAR_RIGHT_1] && !stick_to_plane_) {
        stick_to_plane_ = true;
	is_active_ = true;

        // set current distance to surface as sticking distance
        Eigen::Vector3f plane_pos = Eigen::Vector3f( plane_tf_.getOrigin().x(), plane_tf_.getOrigin().y(), plane_tf_.getOrigin().z() );
        tf::Quaternion plane_q = plane_tf_.getRotation();
        Eigen::Quaternionf plane_rot = Eigen::Quaternionf(plane_q.w(),plane_q.x(),plane_q.y(),plane_q.z());
        Eigen::Vector3f normal = plane_rot*forward;
        normal.normalize();
        Eigen::Vector3f curr_pos = Eigen::Vector3f(mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), mavToWorld_.getOrigin().z());
        sticking_distance_ = -normal.dot(curr_pos-plane_pos);

        // TODO delete
        // eval
        filestream_.open("//usr//stud//grzimek//eval.txt", std::ofstream::out);
        if(!filestream_) {
            std::cout << "Error: could not open file" << std::endl;
        } else {
            std::cout << "Writing to file..." << std::endl;
        }
    }
    if(joy->buttons[PS3_BUTTON_REAR_LEFT_1] && stick_to_plane_) {
        stick_to_plane_ = false;

        // TODO delete
        // eval
        // ...
        if(filestream_) {
            std::cout << "Closing file..." << std::endl;
            filestream_.close();
        }
    }
    // sticking distance
    if(joy->buttons[PS3_BUTTON_CROSS_UP] && abs(sticking_distance_) > 0.5f) {
        sticking_distance_ -= 0.005f;
	is_active_ = true;
    }
    if(joy->buttons[PS3_BUTTON_CROSS_DOWN]) {
        sticking_distance_ += 0.005f;
	is_active_ = true;
    }

    /// axes
    if(std::abs(joy->axes[PS3_AXIS_STICK_RIGHT_UPWARDS]) > STICK_DEADZONE ||
	std::abs(joy->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]) > STICK_DEADZONE ||
	std::abs(joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS]) > STICK_DEADZONE ||
	std::abs(joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS]) > STICK_DEADZONE) {
	is_active_ = true;
    }
    /// translation from controller axis
    float jx = speedX_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_RIGHT_UPWARDS]);
    float jy = speedY_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]);
    float jz = speedZ_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_LEFT_UPWARDS]);
    // yaw from axis
    float jr = speedYaw_ * stick_deadzone_(joy->axes[PS3_AXIS_STICK_LEFT_LEFTWARDS]);

    // save only the latest relative transform in global variable transform_
    transform_.setOrigin( tf::Vector3(jx,jy,jz) );
    tf::Quaternion q;
    q.setRPY(0,0,jr);
    transform_.setRotation(q);
}

void Controller::takeoffAndHover()
{
    std_srvs::Empty::Request request;
    std_srvs::Empty::Response response;
    if(ros::service::call("euroc2/takeoff", request, response)) {
        tf::Vector3 desired_pos = tf::Vector3( mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), 1.0f);
        tf::Quaternion desired_rot = mavToWorld_.getRotation();

        hover_goal_tf_.setOrigin(desired_pos);
        hover_goal_tf_.setRotation(desired_rot);
    }
}

void Controller::processPlaneMsg(const geometry_msgs::TransformStamped::ConstPtr& msg)
{

    // realtive plane transform
    // planeToCam = plane_tf
    plane_tf_.setOrigin( tf::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z) );
    plane_tf_.setRotation( tf::Quaternion(msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w) );

    // plane forward is z should be x, conversion of systems
    // camToSensor
    tf::Quaternion q1; q1.setRPY(0,M_PI/2.0,0);
    tf::Quaternion q2; q2.setRPY(-M_PI/2.0,0,0);
    tf::Quaternion opticalToSensor = q2*q1;
    plane_tf_.setOrigin( tf::Matrix3x3(opticalToSensor)*plane_tf_.getOrigin() );
    plane_tf_.setRotation( opticalToSensor*plane_tf_.getRotation() );
    //br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "euroc_hex/vi_sensor/ground_truth", "plane_Sensor") );

    // correct the relative camera offset
    // planeToMav = plane_tf
    plane_tf_ = sensorToMav_*plane_tf_;

    // transform into global coordinates
    // planeToWorld = plane_tf_
    plane_tf_ = mavToWorld_*plane_tf_;

    // rviz debug
    br_tf_.sendTransform( tf::StampedTransform(plane_tf_, ros::Time::now(), "world", "controller/plane") );
}

void Controller::testPlanes()
{
    //// mav
    // predicted mav tf
    tf::Transform mav_tf = mavToWorld_ * transform_;

    // Eigen conversions
    Eigen::Vector3f mav_pos = Eigen::Vector3f( mav_tf.getOrigin().x(),
                                               mav_tf.getOrigin().y(),
                                               mav_tf.getOrigin().z());

    //// plane
    Eigen::Vector3f plane_pos = Eigen::Vector3f( plane_tf_.getOrigin().x(), plane_tf_.getOrigin().y(), plane_tf_.getOrigin().z() );
    tf::Quaternion plane_q = plane_tf_.getRotation();
    Eigen::Quaternionf plane_rot = Eigen::Quaternionf(plane_q.w(),plane_q.x(),plane_q.y(),plane_q.z());

    //// calculations
    Eigen::Vector3f normal = plane_rot*forward;
    normal.normalize();
    // determine if mav is in front or behind plane normal, take current pos to avoid switching of sides by wrong predictions in mav_tf
    Eigen::Vector3f curr_pos = Eigen::Vector3f(mavToWorld_.getOrigin().x(), mavToWorld_.getOrigin().y(), mavToWorld_.getOrigin().z());

    int facing = normal.dot(curr_pos-plane_pos) >= 0 ? 1 : -1;
    // calculate projected position of the mav onto the plane
    Eigen::Vector3f proj_pos = mav_pos + (facing*sticking_distance_ - normal.dot(mav_pos-plane_pos))*normal;

    // set snapping goal tf
    snap_goal_tf_.setOrigin( tf::Vector3(proj_pos.x(), proj_pos.y(), proj_pos.z()) );
    snap_goal_tf_.setRotation(plane_tf_.getRotation());

    // TODO delete
    // eval
    filestream_ << mavToWorld_.getOrigin().x() << "," << mavToWorld_.getOrigin().y() << "," << mavToWorld_.getOrigin().z() << std::endl;
    filestream_ << proj_pos.x() << "," << proj_pos.y() << "," << proj_pos.z() << std::endl;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "te_joy_controller");
    Controller rc;
    ros::spin();
}
<|endoftext|>


<commit_before>/**************************************************************************
 * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

/* $Id$ */

/*
  Base Class
  Produces the data needed to calculate the quality assurance.
  All data must be mergeable objects.
  Y. Schutz CERN July 2007
*/

// --- ROOT system ---
#include <TSystem.h>
#include <TFile.h>
#include <TList.h>
#include <TTree.h>
#include <TClonesArray.h>

// --- Standard library ---

// --- AliRoot header files ---
#include "AliLog.h"
#include "AliQADataMaker.h"
#include "AliQAChecker.h"
#include "AliESDEvent.h"
#include "AliRawReader.h"

ClassImp(AliQADataMaker)

//____________________________________________________________________________
AliQADataMaker::AliQADataMaker(const char * name, const char * title) :
  TNamed(name, title),
  fOutput(0x0),
  fDetectorDir(0x0),
  fDetectorDirName(""),
  fDigitsQAList(0x0),
  fESDsQAList(0x0),
  fHitsQAList(0x0),
  fRawsQAList(0x0),
  fRecPointsQAList(0x0),
  fSDigitsQAList(0x0),
  fCurrentCycle(-1),
  fCycle(9999999),
  fCycleCounter(0),
  fRun(0)
{
  // ctor
  fDetectorDirName = GetName() ;
}

//____________________________________________________________________________
AliQADataMaker::AliQADataMaker(const AliQADataMaker& qadm) :
  TNamed(qadm.GetName(), qadm.GetTitle()),
  fOutput(qadm.fOutput),
  fDetectorDir(qadm.fDetectorDir),
  fDetectorDirName(qadm.fDetectorDirName),
  fDigitsQAList(qadm.fDigitsQAList),
  fESDsQAList(qadm.fESDsQAList),
  fHitsQAList(qadm.fHitsQAList),
  fRawsQAList(qadm.fRawsQAList),
  fRecPointsQAList(qadm.fRecPointsQAList),
  fSDigitsQAList(qadm.fSDigitsQAList),
  fCurrentCycle(qadm.fCurrentCycle),
  fCycle(qadm.fCycle),
  fCycleCounter(qadm.fCycleCounter),
  fRun(qadm.fRun)
{
  //copy ctor
  fDetectorDirName = GetName() ;
}

//__________________________________________________________________
AliQADataMaker& AliQADataMaker::operator = (const AliQADataMaker& qadm )
{
  // Assignment operator.
  this->~AliQADataMaker();
  new(this) AliQADataMaker(qadm);
  return *this;
}

//____________________________________________________________________________
void AliQADataMaker::EndOfCycle(AliQA::TASKINDEX task)
{
  // Finishes a cycle of QA data acquistion

 TList * list = 0x0 ;

 switch (task) {

  case AliQA::kRAWS:
	list = fRawsQAList ;
  break ;

  case AliQA::kHITS:
	list = fHitsQAList ;
  break ;

  case AliQA::kSDIGITS:
 	list = fSDigitsQAList ;
  break ;

  case AliQA::kDIGITS:
 	list = fDigitsQAList ;
  break ;

   case AliQA::kRECPOINTS:
	list = fRecPointsQAList ;
   break ;

   case AliQA::kTRACKSEGMENTS:
   break ;

   case AliQA::kRECPARTICLES:
   break ;

   case AliQA::kESDS:
	list = fESDsQAList ;
   break ;
  }

 EndOfDetectorCycle(task, list) ;
 TDirectory * subDir = fDetectorDir->GetDirectory(AliQA::GetTaskName(task)) ;
 subDir->cd() ;
 list->Write() ;
}

//____________________________________________________________________________
void AliQADataMaker::Exec(AliQA::TASKINDEX task, TObject * data)
{
  // creates the quality assurance data for the various tasks (Hits, SDigits, Digits, ESDs)

	switch (task) {

		case AliQA::kRAWS:
		{
			AliDebug(1, "Processing Raws QA") ;
			AliRawReader * rawReader = dynamic_cast<AliRawReader *>(data) ;
			if (rawReader)
				MakeRaws(rawReader) ;
			else
			AliError("Wrong data type") ;
			break ;
		}
		case AliQA::kHITS:
		{
			AliDebug(1, "Processing Hits QA") ;
			TClonesArray * arr = dynamic_cast<TClonesArray *>(data) ;
			if (arr) {
				MakeHits(arr) ;
			} else {
				TTree * tree = dynamic_cast<TTree *>(data) ;
				if (tree) {
					MakeHits(tree) ;
				} else {
					AliWarning("data are neither a TClonesArray nor a TTree") ;
				}
			}
			break ;
		}
		case AliQA::kSDIGITS:
		{
			AliDebug(1, "Processing SDigits QA") ;
			TClonesArray * arr = dynamic_cast<TClonesArray *>(data) ;
			if (arr) {
				MakeSDigits(arr) ;
			} else {
				TTree * tree = dynamic_cast<TTree *>(data) ;
				if (tree) {
					MakeSDigits(tree) ;
				} else {
					AliWarning("data are neither a TClonesArray nor a TTree") ;
				}
			}
			break ;
		}
		case AliQA::kDIGITS:
		{
			AliDebug(1, "Processing Digits QA") ;
			TClonesArray * arr = dynamic_cast<TClonesArray *>(data) ;
			if (arr) {
				MakeDigits(arr) ;
			} else {
				TTree * tree = dynamic_cast<TTree *>(data) ;
				if (tree) {
					MakeDigits(tree) ;
				} else {
					AliWarning("data are neither a TClonesArray nor a TTree") ;
				}
			}
			break ;
		}
		case AliQA::kRECPOINTS:
		{
			AliDebug(1, "Processing RecPoints QA") ;
			TTree * tree = dynamic_cast<TTree *>(data) ;
			if (tree) {
				MakeRecPoints(tree) ;
			} else {
				AliWarning("data are not a TTree") ;
			}
			break ;
		}
		case AliQA::kTRACKSEGMENTS:
			AliInfo("Processing Track Segments QA: not existing anymore") ;
		//       MakeTrackSegments(ts) ;
		break ;

		case AliQA::kRECPARTICLES:
			AliInfo("Processing RecParticles QA: not existing anymore") ;
			//       MakeRecParticles(recpar) ;
		break ;
		case AliQA::kESDS:
		{
			AliDebug(1, "Processing ESDs QA") ;
			AliESDEvent * esd = dynamic_cast<AliESDEvent *>(data) ;
			if (esd)
				MakeESDs(esd) ;
			else
				AliError("Wrong type of esd container") ;
			break ;
		}
	}
}

//____________________________________________________________________________
void AliQADataMaker::Finish() const
{
  // write to the output File
  fOutput->Close() ;
}

//____________________________________________________________________________
TList *  AliQADataMaker::Init(AliQA::TASKINDEX task, Int_t run, Int_t cycles)
{
  // general intialisation

  fRun = run ;
  if (cycles > 0)
    SetCycle(cycles) ;

  switch (task) {
  case AliQA::kRAWS:
   {
	fRawsQAList = new TList() ;
    InitRaws() ;
	return fRawsQAList ;
    break ;
   }
  case AliQA::kHITS:
   {
	fHitsQAList = new TList() ;
    InitHits() ;
	return fHitsQAList ;
    break ;
   }
  case AliQA::kSDIGITS:
   {
	fSDigitsQAList = new TList() ;
    InitSDigits() ;
	return fSDigitsQAList ;
    break ;
   }
  case AliQA::kDIGITS:
   {
	fDigitsQAList = new TList();
	InitDigits() ;
	return fDigitsQAList ;
	break ;
   }
  case AliQA::kRECPOINTS:
   {
	fRecPointsQAList = new TList() ;
    InitRecPoints() ;
	return fRecPointsQAList ;
    break ;
  }
  case AliQA::kTRACKSEGMENTS:
//  InitTrackSegments() ;
    break ;

  case AliQA::kRECPARTICLES:
//    InitRecParticles() ;
    break ;

  case AliQA::kESDS:
   {
	fESDsQAList = new TList() ;
	InitESDs() ;
	return fRecPointsQAList ;
    break ;
   }
  }
  return 0x0 ;
}

//____________________________________________________________________________
void AliQADataMaker::Init(AliQA::TASKINDEX task, TList * list, Int_t run, Int_t cycles)
{
  // Intialisation by passing the list of QA data booked elsewhere

  fRun = run ;
  if (cycles > 0)
    SetCycle(cycles) ;

  switch (task) {
  case AliQA::kRAWS:
   {
	fRawsQAList = list ;
    break ;
   }
  case AliQA::kHITS:
   {
	fHitsQAList = list ;
    break ;
   }
  case AliQA::kSDIGITS:
   {
	fSDigitsQAList = list ;
    break ;
   }
  case AliQA::kDIGITS:
   {
	fDigitsQAList = list ;
	break ;
   }
  case AliQA::kRECPOINTS:
   {
	fRecPointsQAList = list ;
    break ;
  }
  case AliQA::kTRACKSEGMENTS:
    break ;

  case AliQA::kRECPARTICLES:
    break ;

  case AliQA::kESDS:
   {
	fESDsQAList = list ;
    break ;
   }
  }
}

//____________________________________________________________________________
void AliQADataMaker::Reset()
{
  // Resets defaut value of data members
  fCurrentCycle = -1 ;
  fCycleCounter = 0 ;
}

//____________________________________________________________________________
void AliQADataMaker::StartOfCycle(AliQA::TASKINDEX task, const Bool_t sameCycle)
{
  // Finishes a cycle of QA data acquistion

 if ( !sameCycle ) {
	ResetCycle() ;
	if (fOutput)
		fOutput->Close() ;
	fOutput = AliQA::GetQADataFile(GetName(), fRun, fCurrentCycle) ;
}
 AliInfo(Form(" Run %d Cycle %d task %s file %s",
	fRun, fCurrentCycle, AliQA::GetTaskName(task).Data(), fOutput->GetName() )) ;

 fDetectorDir = fOutput->GetDirectory(GetDetectorDirName()) ;
 if (!fDetectorDir)
   fDetectorDir = fOutput->mkdir(GetDetectorDirName()) ;

 TDirectory * subDir = fDetectorDir->GetDirectory(AliQA::GetTaskName(task)) ;
 if (!subDir)
   subDir = fDetectorDir->mkdir(AliQA::GetTaskName(task)) ;
 subDir->cd() ;

  TList * list = 0x0 ;

  switch (task) {
  case AliQA::kRAWS:
	list = fRawsQAList ;
    break ;

  case AliQA::kHITS:
	list = fHitsQAList ;
    break ;

  case AliQA::kSDIGITS:
	list = fSDigitsQAList ;
    break ;

  case AliQA::kDIGITS:
	list = fDigitsQAList ;
	break ;

  case AliQA::kRECPOINTS:
	list = fRecPointsQAList ;
	break ;

  case AliQA::kTRACKSEGMENTS:
    break ;

  case AliQA::kRECPARTICLES:
    break ;

  case AliQA::kESDS:
  	list = fESDsQAList ;
    break ;
  }

 TIter next(list) ;
 TH1 * h ;
 while ( (h = dynamic_cast<TH1 *>(next())) )
    h->Reset() ;

 StartOfDetectorCycle() ;

}
<commit_msg>Create output file when firs cycle = -1 (first first cycle)<commit_after>/**************************************************************************
 * Copyright(c) 1998-1999, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

/* $Id$ */

/*
  Base Class
  Produces the data needed to calculate the quality assurance.
  All data must be mergeable objects.
  Y. Schutz CERN July 2007
*/

// --- ROOT system ---
#include <TSystem.h>
#include <TFile.h>
#include <TList.h>
#include <TTree.h>
#include <TClonesArray.h>

// --- Standard library ---

// --- AliRoot header files ---
#include "AliLog.h"
#include "AliQADataMaker.h"
#include "AliQAChecker.h"
#include "AliESDEvent.h"
#include "AliRawReader.h"

ClassImp(AliQADataMaker)

//____________________________________________________________________________
AliQADataMaker::AliQADataMaker(const char * name, const char * title) :
  TNamed(name, title),
  fOutput(0x0),
  fDetectorDir(0x0),
  fDetectorDirName(""),
  fDigitsQAList(0x0),
  fESDsQAList(0x0),
  fHitsQAList(0x0),
  fRawsQAList(0x0),
  fRecPointsQAList(0x0),
  fSDigitsQAList(0x0),
  fCurrentCycle(-1),
  fCycle(9999999),
  fCycleCounter(0),
  fRun(0)
{
  // ctor
  fDetectorDirName = GetName() ;
}

//____________________________________________________________________________
AliQADataMaker::AliQADataMaker(const AliQADataMaker& qadm) :
  TNamed(qadm.GetName(), qadm.GetTitle()),
  fOutput(qadm.fOutput),
  fDetectorDir(qadm.fDetectorDir),
  fDetectorDirName(qadm.fDetectorDirName),
  fDigitsQAList(qadm.fDigitsQAList),
  fESDsQAList(qadm.fESDsQAList),
  fHitsQAList(qadm.fHitsQAList),
  fRawsQAList(qadm.fRawsQAList),
  fRecPointsQAList(qadm.fRecPointsQAList),
  fSDigitsQAList(qadm.fSDigitsQAList),
  fCurrentCycle(qadm.fCurrentCycle),
  fCycle(qadm.fCycle),
  fCycleCounter(qadm.fCycleCounter),
  fRun(qadm.fRun)
{
  //copy ctor
  fDetectorDirName = GetName() ;
}

//__________________________________________________________________
AliQADataMaker& AliQADataMaker::operator = (const AliQADataMaker& qadm )
{
  // Assignment operator.
  this->~AliQADataMaker();
  new(this) AliQADataMaker(qadm);
  return *this;
}

//____________________________________________________________________________
void AliQADataMaker::EndOfCycle(AliQA::TASKINDEX task)
{
  // Finishes a cycle of QA data acquistion

 TList * list = 0x0 ;

 switch (task) {

  case AliQA::kRAWS:
	list = fRawsQAList ;
  break ;

  case AliQA::kHITS:
	list = fHitsQAList ;
  break ;

  case AliQA::kSDIGITS:
 	list = fSDigitsQAList ;
  break ;

  case AliQA::kDIGITS:
 	list = fDigitsQAList ;
  break ;

   case AliQA::kRECPOINTS:
	list = fRecPointsQAList ;
   break ;

   case AliQA::kTRACKSEGMENTS:
   break ;

   case AliQA::kRECPARTICLES:
   break ;

   case AliQA::kESDS:
	list = fESDsQAList ;
   break ;
  }

 EndOfDetectorCycle(task, list) ;
 TDirectory * subDir = fDetectorDir->GetDirectory(AliQA::GetTaskName(task)) ;
 subDir->cd() ;
 list->Write() ;
}

//____________________________________________________________________________
void AliQADataMaker::Exec(AliQA::TASKINDEX task, TObject * data)
{
  // creates the quality assurance data for the various tasks (Hits, SDigits, Digits, ESDs)

	switch (task) {

		case AliQA::kRAWS:
		{
			AliDebug(1, "Processing Raws QA") ;
			AliRawReader * rawReader = dynamic_cast<AliRawReader *>(data) ;
			if (rawReader)
				MakeRaws(rawReader) ;
			else
			AliError("Wrong data type") ;
			break ;
		}
		case AliQA::kHITS:
		{
			AliDebug(1, "Processing Hits QA") ;
			TClonesArray * arr = dynamic_cast<TClonesArray *>(data) ;
			if (arr) {
				MakeHits(arr) ;
			} else {
				TTree * tree = dynamic_cast<TTree *>(data) ;
				if (tree) {
					MakeHits(tree) ;
				} else {
					AliWarning("data are neither a TClonesArray nor a TTree") ;
				}
			}
			break ;
		}
		case AliQA::kSDIGITS:
		{
			AliDebug(1, "Processing SDigits QA") ;
			TClonesArray * arr = dynamic_cast<TClonesArray *>(data) ;
			if (arr) {
				MakeSDigits(arr) ;
			} else {
				TTree * tree = dynamic_cast<TTree *>(data) ;
				if (tree) {
					MakeSDigits(tree) ;
				} else {
					AliWarning("data are neither a TClonesArray nor a TTree") ;
				}
			}
			break ;
		}
		case AliQA::kDIGITS:
		{
			AliDebug(1, "Processing Digits QA") ;
			TClonesArray * arr = dynamic_cast<TClonesArray *>(data) ;
			if (arr) {
				MakeDigits(arr) ;
			} else {
				TTree * tree = dynamic_cast<TTree *>(data) ;
				if (tree) {
					MakeDigits(tree) ;
				} else {
					AliWarning("data are neither a TClonesArray nor a TTree") ;
				}
			}
			break ;
		}
		case AliQA::kRECPOINTS:
		{
			AliDebug(1, "Processing RecPoints QA") ;
			TTree * tree = dynamic_cast<TTree *>(data) ;
			if (tree) {
				MakeRecPoints(tree) ;
			} else {
				AliWarning("data are not a TTree") ;
			}
			break ;
		}
		case AliQA::kTRACKSEGMENTS:
			AliInfo("Processing Track Segments QA: not existing anymore") ;
		//       MakeTrackSegments(ts) ;
		break ;

		case AliQA::kRECPARTICLES:
			AliInfo("Processing RecParticles QA: not existing anymore") ;
			//       MakeRecParticles(recpar) ;
		break ;
		case AliQA::kESDS:
		{
			AliDebug(1, "Processing ESDs QA") ;
			AliESDEvent * esd = dynamic_cast<AliESDEvent *>(data) ;
			if (esd)
				MakeESDs(esd) ;
			else
				AliError("Wrong type of esd container") ;
			break ;
		}
	}
}

//____________________________________________________________________________
void AliQADataMaker::Finish() const
{
  // write to the output File
  fOutput->Close() ;
}

//____________________________________________________________________________
TList *  AliQADataMaker::Init(AliQA::TASKINDEX task, Int_t run, Int_t cycles)
{
  // general intialisation

  fRun = run ;
  if (cycles > 0)
    SetCycle(cycles) ;

  switch (task) {
  case AliQA::kRAWS:
   {
	fRawsQAList = new TList() ;
    InitRaws() ;
	return fRawsQAList ;
    break ;
   }
  case AliQA::kHITS:
   {
	fHitsQAList = new TList() ;
    InitHits() ;
	return fHitsQAList ;
    break ;
   }
  case AliQA::kSDIGITS:
   {
	fSDigitsQAList = new TList() ;
    InitSDigits() ;
	return fSDigitsQAList ;
    break ;
   }
  case AliQA::kDIGITS:
   {
	fDigitsQAList = new TList();
	InitDigits() ;
	return fDigitsQAList ;
	break ;
   }
  case AliQA::kRECPOINTS:
   {
	fRecPointsQAList = new TList() ;
    InitRecPoints() ;
	return fRecPointsQAList ;
    break ;
  }
  case AliQA::kTRACKSEGMENTS:
//  InitTrackSegments() ;
    break ;

  case AliQA::kRECPARTICLES:
//    InitRecParticles() ;
    break ;

  case AliQA::kESDS:
   {
	fESDsQAList = new TList() ;
	InitESDs() ;
	return fRecPointsQAList ;
    break ;
   }
  }
  return 0x0 ;
}

//____________________________________________________________________________
void AliQADataMaker::Init(AliQA::TASKINDEX task, TList * list, Int_t run, Int_t cycles)
{
  // Intialisation by passing the list of QA data booked elsewhere

  fRun = run ;
  if (cycles > 0)
    SetCycle(cycles) ;

  switch (task) {
  case AliQA::kRAWS:
   {
	fRawsQAList = list ;
    break ;
   }
  case AliQA::kHITS:
   {
	fHitsQAList = list ;
    break ;
   }
  case AliQA::kSDIGITS:
   {
	fSDigitsQAList = list ;
    break ;
   }
  case AliQA::kDIGITS:
   {
	fDigitsQAList = list ;
	break ;
   }
  case AliQA::kRECPOINTS:
   {
	fRecPointsQAList = list ;
    break ;
  }
  case AliQA::kTRACKSEGMENTS:
    break ;

  case AliQA::kRECPARTICLES:
    break ;

  case AliQA::kESDS:
   {
	fESDsQAList = list ;
    break ;
   }
  }
}

//____________________________________________________________________________
void AliQADataMaker::Reset()
{
  // Resets defaut value of data members
  fCurrentCycle = -1 ;
  fCycleCounter = 0 ;
}

//____________________________________________________________________________
void AliQADataMaker::StartOfCycle(AliQA::TASKINDEX task, const Bool_t sameCycle)
{
  // Finishes a cycle of QA data acquistion

 if ( !sameCycle || fCurrentCycle == -1) {
	ResetCycle() ;
	if (fOutput)
		fOutput->Close() ;
	fOutput = AliQA::GetQADataFile(GetName(), fRun, fCurrentCycle) ;
}
 AliInfo(Form(" Run %d Cycle %d task %s file %s",
	fRun, fCurrentCycle, AliQA::GetTaskName(task).Data(), fOutput->GetName() )) ;

 fDetectorDir = fOutput->GetDirectory(GetDetectorDirName()) ;
 if (!fDetectorDir)
   fDetectorDir = fOutput->mkdir(GetDetectorDirName()) ;

 TDirectory * subDir = fDetectorDir->GetDirectory(AliQA::GetTaskName(task)) ;
 if (!subDir)
   subDir = fDetectorDir->mkdir(AliQA::GetTaskName(task)) ;
 subDir->cd() ;

  TList * list = 0x0 ;

  switch (task) {
  case AliQA::kRAWS:
	list = fRawsQAList ;
    break ;

  case AliQA::kHITS:
	list = fHitsQAList ;
    break ;

  case AliQA::kSDIGITS:
	list = fSDigitsQAList ;
    break ;

  case AliQA::kDIGITS:
	list = fDigitsQAList ;
	break ;

  case AliQA::kRECPOINTS:
	list = fRecPointsQAList ;
	break ;

  case AliQA::kTRACKSEGMENTS:
    break ;

  case AliQA::kRECPARTICLES:
    break ;

  case AliQA::kESDS:
  	list = fESDsQAList ;
    break ;
  }

 TIter next(list) ;
 TH1 * h ;
 while ( (h = dynamic_cast<TH1 *>(next())) )
    h->Reset() ;

 StartOfDetectorCycle() ;

}
<|endoftext|>