<commit_before>// Copyright (C) 2017 Elviss Strazdins
// This file is part of the Ouzel engine.

#include "core/CompileConfig.h"

#if OUZEL_PLATFORM_ANDROID && OUZEL_SUPPORTS_OPENGL

#include "RenderDeviceOGLAndroid.hpp"
#include "graphics/opengl/ResourceOGL.hpp"
#include "core/android/WindowAndroid.hpp"
#include "core/Engine.hpp"
#include "utils/Log.hpp"

namespace ouzel
{
    namespace graphics
    {
        RenderDeviceOGLAndroid::~RenderDeviceOGLAndroid()
        {
            running = false;
            flushCommands();
            if (renderThread.joinable()) renderThread.join();

            if (context)
            {
                if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
                {
                    Log(Log::Level::ERR) << "Failed to unset EGL context";
                }

                if (!eglDestroyContext(display, context))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL context";
                }
            }

            if (surface)
            {
                if (!eglDestroySurface(display, surface))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL surface";
                }
            }

            if (display)
            {
                if (!eglTerminate(display))
                {
                    Log(Log::Level::ERR) << "Failed to terminate EGL";
                }
            }
        }

        bool RenderDeviceOGLAndroid::init(Window* newWindow,
                                          const Size2&,
                                          uint32_t newSampleCount,
                                          Texture::Filter newTextureFilter,
                                          uint32_t newMaxAnisotropy,
                                          bool newVerticalSync,
                                          bool newDepth,
                                          bool newDebugRenderer)
        {
            display = eglGetDisplay(EGL_DEFAULT_DISPLAY);

            if (!display)
            {
                Log(Log::Level::ERR) << "Failed to get display";
                return false;
            }

            if (!eglInitialize(display, nullptr, nullptr))
            {
                Log(Log::Level::ERR) << "Failed to initialize EGL";
                return false;
            }

            const EGLint attributeList[] =
            {
                EGL_RED_SIZE, 8,
                EGL_GREEN_SIZE, 8,
                EGL_BLUE_SIZE, 8,
                EGL_ALPHA_SIZE, 8,
                EGL_DEPTH_SIZE, newDepth ? 24 : 0,
                EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
                EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
                EGL_SAMPLE_BUFFERS, (newSampleCount > 1) ? 1 : 0,
                EGL_SAMPLES, static_cast<int>(newSampleCount),
                EGL_NONE
            };
            EGLConfig config;
            EGLint numConfig;
            if (!eglChooseConfig(display, attributeList, &config, 1, &numConfig))
            {
                Log(Log::Level::ERR) << "Failed to choose EGL config";
                return false;
            }

            if (!eglBindAPI(EGL_OPENGL_ES_API))
            {
                Log(Log::Level::ERR) << "Failed to bind OpenGL ES API";
                return false;
            }

            EGLint format;
            if (!eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format))
            {
                Log(Log::Level::ERR) << "Failed to get config attribute " << eglGetError();
                return false;
            }

            WindowAndroid* windowAndroid = static_cast<WindowAndroid*>(newWindow);

            ANativeWindow_setBuffersGeometry(windowAndroid->getNativeWindow(), 0, 0, format);

            surface = eglCreateWindowSurface(display, config, windowAndroid->getNativeWindow(), nullptr);
            if (surface == EGL_NO_SURFACE)
            {
                Log(Log::Level::ERR) << "Failed to create EGL window surface";
                return false;
            }

            for (EGLint version = 3; version >= 2; --version)
            {
                std::vector<EGLint> contextAttributes =
                {
                    EGL_CONTEXT_CLIENT_VERSION, version
                };

                if (newDebugRenderer)
                {
                    contextAttributes.push_back(EGL_CONTEXT_FLAGS_KHR);
                    contextAttributes.push_back(EGL_CONTEXT_OPENGL_DEBUG_BIT_KHR);
                }

                contextAttributes.push_back(EGL_NONE);

                context = eglCreateContext(display, config, EGL_NO_CONTEXT, contextAttributes.data());

                if (context != EGL_NO_CONTEXT)
                {
                    apiMajorVersion = version;
                    apiMinorVersion = 0;
                    Log(Log::Level::INFO) << "EGL OpenGL ES " << version << " context created";
                    break;
                }
            }

            if (context == EGL_NO_CONTEXT)
            {
                Log(Log::Level::ERR) << "Failed to create EGL context";
                return false;
            }

            if (!eglMakeCurrent(display, surface, surface, context))
            {
                Log(Log::Level::ERR) << "Failed to set current EGL context";
                return false;
            }

            if (!eglSwapInterval(display, newVerticalSync ? 1 : 0))
            {
                Log(Log::Level::ERR) << "Failed to set EGL frame interval";
                return false;
            }

            EGLint surfaceWidth;
            EGLint surfaceHeight;

            if (!eglQuerySurface(display, surface, EGL_WIDTH, &surfaceWidth) ||
                !eglQuerySurface(display, surface, EGL_HEIGHT, &surfaceHeight))
            {
                Log(Log::Level::ERR) << "Failed to get query window size " <<  eglGetError();
                return false;
            }

            frameBufferWidth = surfaceWidth;
            frameBufferHeight = surfaceHeight;

            Size2 backBufferSize = Size2(static_cast<float>(frameBufferWidth),
                                         static_cast<float>(frameBufferHeight));

            newWindow->setSize(backBufferSize / newWindow->getContentScale());

            if (!RenderDeviceOGL::init(newWindow,
                                       backBufferSize,
                                       newSampleCount,
                                       newTextureFilter,
                                       newMaxAnisotropy,
                                       newVerticalSync,
                                       newDepth,
                                       newDebugRenderer))
            {
                return false;
            }

            if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
            {
                Log(Log::Level::ERR) << "Failed to unset EGL context";
            }

            running = true;
            renderThread = std::thread(&RenderDeviceOGLAndroid::main, this);

            return true;
        }

        bool RenderDeviceOGLAndroid::reload()
        {
            if (!eglInitialize(display, nullptr, nullptr))
            {
                Log(Log::Level::ERR) << "Failed to initialize EGL";
                return false;
            }

            const EGLint attributeList[] =
            {
                EGL_RED_SIZE, 8,
                EGL_GREEN_SIZE, 8,
                EGL_BLUE_SIZE, 8,
                EGL_ALPHA_SIZE, 8,
                EGL_DEPTH_SIZE, depth ? 24 : 0,
                EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
                EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
                EGL_SAMPLE_BUFFERS, (sampleCount > 1) ? 1 : 0,
                EGL_SAMPLES, static_cast<int>(sampleCount),
                EGL_NONE
            };
            EGLConfig config;
            EGLint numConfig;
            if (!eglChooseConfig(display, attributeList, &config, 1, &numConfig))
            {
                Log(Log::Level::ERR) << "Failed to choose EGL config";
                return false;
            }

            if (!eglBindAPI(EGL_OPENGL_ES_API))
            {
                Log(Log::Level::ERR) << "Failed to bind OpenGL ES API";
                return false;
            }

            EGLint format;
            if (!eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format))
            {
                Log(Log::Level::ERR) << "Failed to get config attribute " << eglGetError();
                return false;
            }

            WindowAndroid* windowAndroid = static_cast<WindowAndroid*>(sharedEngine->getWindow());

            ANativeWindow_setBuffersGeometry(windowAndroid->getNativeWindow(), 0, 0, format);

            surface = eglCreateWindowSurface(display, config, windowAndroid->getNativeWindow(), nullptr);
            if (surface == EGL_NO_SURFACE)
            {
                Log(Log::Level::ERR) << "Failed to create EGL window surface";
                return false;
            }

            for (EGLint version = 3; version >= 2; --version)
            {
                std::vector<EGLint> contextAttributes =
                {
                    EGL_CONTEXT_CLIENT_VERSION, version
                };

                if (debugRenderer)
                {
                    contextAttributes.push_back(EGL_CONTEXT_FLAGS_KHR);
                    contextAttributes.push_back(EGL_CONTEXT_OPENGL_DEBUG_BIT_KHR);
                }

                contextAttributes.push_back(EGL_NONE);

                context = eglCreateContext(display, config, EGL_NO_CONTEXT, contextAttributes.data());

                if (context != EGL_NO_CONTEXT)
                {
                    apiMajorVersion = version;
                    apiMinorVersion = 0;
                    Log(Log::Level::INFO) << "EGL OpenGL ES " << version << " context created";
                    break;
                }
            }

            if (context == EGL_NO_CONTEXT)
            {
                Log(Log::Level::ERR) << "Failed to create EGL context";
                return false;
            }

            if (!eglMakeCurrent(display, surface, surface, context))
            {
                Log(Log::Level::ERR) << "Failed to set current EGL context";
                return false;
            }

            if (!eglSwapInterval(display, verticalSync ? 1 : 0))
            {
                Log(Log::Level::ERR) << "Failed to set EGL frame interval";
                return false;
            }

            EGLint surfaceWidth;
            EGLint surfaceHeight;

            if (!eglQuerySurface(display, surface, EGL_WIDTH, &surfaceWidth) ||
                !eglQuerySurface(display, surface, EGL_HEIGHT, &surfaceHeight))
            {
                Log(Log::Level::ERR) << "Failed to get query window size " <<  eglGetError();
                return false;
            }

            frameBufferWidth = surfaceWidth;
            frameBufferHeight = surfaceHeight;

            Size2 backBufferSize = Size2(static_cast<float>(frameBufferWidth),
                                         static_cast<float>(frameBufferHeight));

            windowAndroid->setSize(backBufferSize / windowAndroid->getContentScale());

            for (const std::unique_ptr<Resource>& resource : resources)
            {
                ResourceOGL* resourceOGL = reinterpret_cast<ResourceOGL*>(resource.get());
                if (!resourceOGL->reload())
                {
                    return false;
                }
            }

            if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
            {
                Log(Log::Level::ERR) << "Failed to unset EGL context";
            }

            return true;
        }

        bool RenderDeviceOGLAndroid::destroy()
        {
            if (context)
            {
                if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
                {
                    Log(Log::Level::ERR) << "Failed to unset EGL context";
                }

                if (!eglDestroyContext(display, context))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL context";
                }

                context = nullptr;
            }

            if (surface)
            {
                if (!eglDestroySurface(display, surface))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL surface";
                }

                surface = nullptr;
            }

            return true;
        }

        bool RenderDeviceOGLAndroid::lockContext()
        {
            if (!eglMakeCurrent(display, surface, surface, context))
            {
                Log(Log::Level::ERR) << "Failed to set current EGL context, error: " << eglGetError();
                return false;
            }

            return true;
        }

        bool RenderDeviceOGLAndroid::swapBuffers()
        {
            if (eglSwapBuffers(display, surface) != EGL_TRUE)
            {
                Log(Log::Level::ERR) << "Failed to swap buffers " << eglGetError();
                return false;
            }

            return true;
        }

        void RenderDeviceOGLAndroid::main()
        {
            while (running)
            {
                process();
            }

            if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
            {
                Log(Log::Level::ERR) << "Failed to unset EGL context, error: " << eglGetError();
            }
        }
    } // namespace graphics
} // namespace ouzel

#endif
<commit_msg>Stop the render thread before reinitializing the context<commit_after>// Copyright (C) 2017 Elviss Strazdins
// This file is part of the Ouzel engine.

#include "core/CompileConfig.h"

#if OUZEL_PLATFORM_ANDROID && OUZEL_SUPPORTS_OPENGL

#include "RenderDeviceOGLAndroid.hpp"
#include "graphics/opengl/ResourceOGL.hpp"
#include "core/android/WindowAndroid.hpp"
#include "core/Engine.hpp"
#include "utils/Log.hpp"

namespace ouzel
{
    namespace graphics
    {
        RenderDeviceOGLAndroid::~RenderDeviceOGLAndroid()
        {
            running = false;
            flushCommands();
            if (renderThread.joinable()) renderThread.join();

            if (context)
            {
                if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
                {
                    Log(Log::Level::ERR) << "Failed to unset EGL context";
                }

                if (!eglDestroyContext(display, context))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL context";
                }
            }

            if (surface)
            {
                if (!eglDestroySurface(display, surface))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL surface";
                }
            }

            if (display)
            {
                if (!eglTerminate(display))
                {
                    Log(Log::Level::ERR) << "Failed to terminate EGL";
                }
            }
        }

        bool RenderDeviceOGLAndroid::init(Window* newWindow,
                                          const Size2&,
                                          uint32_t newSampleCount,
                                          Texture::Filter newTextureFilter,
                                          uint32_t newMaxAnisotropy,
                                          bool newVerticalSync,
                                          bool newDepth,
                                          bool newDebugRenderer)
        {
            display = eglGetDisplay(EGL_DEFAULT_DISPLAY);

            if (!display)
            {
                Log(Log::Level::ERR) << "Failed to get display";
                return false;
            }

            if (!eglInitialize(display, nullptr, nullptr))
            {
                Log(Log::Level::ERR) << "Failed to initialize EGL";
                return false;
            }

            const EGLint attributeList[] =
            {
                EGL_RED_SIZE, 8,
                EGL_GREEN_SIZE, 8,
                EGL_BLUE_SIZE, 8,
                EGL_ALPHA_SIZE, 8,
                EGL_DEPTH_SIZE, newDepth ? 24 : 0,
                EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
                EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
                EGL_SAMPLE_BUFFERS, (newSampleCount > 1) ? 1 : 0,
                EGL_SAMPLES, static_cast<int>(newSampleCount),
                EGL_NONE
            };
            EGLConfig config;
            EGLint numConfig;
            if (!eglChooseConfig(display, attributeList, &config, 1, &numConfig))
            {
                Log(Log::Level::ERR) << "Failed to choose EGL config";
                return false;
            }

            if (!eglBindAPI(EGL_OPENGL_ES_API))
            {
                Log(Log::Level::ERR) << "Failed to bind OpenGL ES API";
                return false;
            }

            EGLint format;
            if (!eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format))
            {
                Log(Log::Level::ERR) << "Failed to get config attribute " << eglGetError();
                return false;
            }

            WindowAndroid* windowAndroid = static_cast<WindowAndroid*>(newWindow);

            ANativeWindow_setBuffersGeometry(windowAndroid->getNativeWindow(), 0, 0, format);

            surface = eglCreateWindowSurface(display, config, windowAndroid->getNativeWindow(), nullptr);
            if (surface == EGL_NO_SURFACE)
            {
                Log(Log::Level::ERR) << "Failed to create EGL window surface";
                return false;
            }

            for (EGLint version = 3; version >= 2; --version)
            {
                std::vector<EGLint> contextAttributes =
                {
                    EGL_CONTEXT_CLIENT_VERSION, version
                };

                if (newDebugRenderer)
                {
                    contextAttributes.push_back(EGL_CONTEXT_FLAGS_KHR);
                    contextAttributes.push_back(EGL_CONTEXT_OPENGL_DEBUG_BIT_KHR);
                }

                contextAttributes.push_back(EGL_NONE);

                context = eglCreateContext(display, config, EGL_NO_CONTEXT, contextAttributes.data());

                if (context != EGL_NO_CONTEXT)
                {
                    apiMajorVersion = version;
                    apiMinorVersion = 0;
                    Log(Log::Level::INFO) << "EGL OpenGL ES " << version << " context created";
                    break;
                }
            }

            if (context == EGL_NO_CONTEXT)
            {
                Log(Log::Level::ERR) << "Failed to create EGL context";
                return false;
            }

            if (!eglMakeCurrent(display, surface, surface, context))
            {
                Log(Log::Level::ERR) << "Failed to set current EGL context";
                return false;
            }

            if (!eglSwapInterval(display, newVerticalSync ? 1 : 0))
            {
                Log(Log::Level::ERR) << "Failed to set EGL frame interval";
                return false;
            }

            EGLint surfaceWidth;
            EGLint surfaceHeight;

            if (!eglQuerySurface(display, surface, EGL_WIDTH, &surfaceWidth) ||
                !eglQuerySurface(display, surface, EGL_HEIGHT, &surfaceHeight))
            {
                Log(Log::Level::ERR) << "Failed to get query window size " <<  eglGetError();
                return false;
            }

            frameBufferWidth = surfaceWidth;
            frameBufferHeight = surfaceHeight;

            Size2 backBufferSize = Size2(static_cast<float>(frameBufferWidth),
                                         static_cast<float>(frameBufferHeight));

            newWindow->setSize(backBufferSize / newWindow->getContentScale());

            if (!RenderDeviceOGL::init(newWindow,
                                       backBufferSize,
                                       newSampleCount,
                                       newTextureFilter,
                                       newMaxAnisotropy,
                                       newVerticalSync,
                                       newDepth,
                                       newDebugRenderer))
            {
                return false;
            }

            if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
            {
                Log(Log::Level::ERR) << "Failed to unset EGL context";
            }

            running = true;
            renderThread = std::thread(&RenderDeviceOGLAndroid::main, this);

            return true;
        }

        bool RenderDeviceOGLAndroid::reload()
        {
            running = false;
            flushCommands();
            if (renderThread.joinable()) renderThread.join();

            if (!eglInitialize(display, nullptr, nullptr))
            {
                Log(Log::Level::ERR) << "Failed to initialize EGL";
                return false;
            }

            const EGLint attributeList[] =
            {
                EGL_RED_SIZE, 8,
                EGL_GREEN_SIZE, 8,
                EGL_BLUE_SIZE, 8,
                EGL_ALPHA_SIZE, 8,
                EGL_DEPTH_SIZE, depth ? 24 : 0,
                EGL_RENDERABLE_TYPE, EGL_OPENGL_ES2_BIT,
                EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
                EGL_SAMPLE_BUFFERS, (sampleCount > 1) ? 1 : 0,
                EGL_SAMPLES, static_cast<int>(sampleCount),
                EGL_NONE
            };
            EGLConfig config;
            EGLint numConfig;
            if (!eglChooseConfig(display, attributeList, &config, 1, &numConfig))
            {
                Log(Log::Level::ERR) << "Failed to choose EGL config";
                return false;
            }

            if (!eglBindAPI(EGL_OPENGL_ES_API))
            {
                Log(Log::Level::ERR) << "Failed to bind OpenGL ES API";
                return false;
            }

            EGLint format;
            if (!eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format))
            {
                Log(Log::Level::ERR) << "Failed to get config attribute " << eglGetError();
                return false;
            }

            WindowAndroid* windowAndroid = static_cast<WindowAndroid*>(sharedEngine->getWindow());

            ANativeWindow_setBuffersGeometry(windowAndroid->getNativeWindow(), 0, 0, format);

            surface = eglCreateWindowSurface(display, config, windowAndroid->getNativeWindow(), nullptr);
            if (surface == EGL_NO_SURFACE)
            {
                Log(Log::Level::ERR) << "Failed to create EGL window surface";
                return false;
            }

            for (EGLint version = 3; version >= 2; --version)
            {
                std::vector<EGLint> contextAttributes =
                {
                    EGL_CONTEXT_CLIENT_VERSION, version
                };

                if (debugRenderer)
                {
                    contextAttributes.push_back(EGL_CONTEXT_FLAGS_KHR);
                    contextAttributes.push_back(EGL_CONTEXT_OPENGL_DEBUG_BIT_KHR);
                }

                contextAttributes.push_back(EGL_NONE);

                context = eglCreateContext(display, config, EGL_NO_CONTEXT, contextAttributes.data());

                if (context != EGL_NO_CONTEXT)
                {
                    apiMajorVersion = version;
                    apiMinorVersion = 0;
                    Log(Log::Level::INFO) << "EGL OpenGL ES " << version << " context created";
                    break;
                }
            }

            if (context == EGL_NO_CONTEXT)
            {
                Log(Log::Level::ERR) << "Failed to create EGL context";
                return false;
            }

            if (!eglMakeCurrent(display, surface, surface, context))
            {
                Log(Log::Level::ERR) << "Failed to set current EGL context";
                return false;
            }

            if (!eglSwapInterval(display, verticalSync ? 1 : 0))
            {
                Log(Log::Level::ERR) << "Failed to set EGL frame interval";
                return false;
            }

            EGLint surfaceWidth;
            EGLint surfaceHeight;

            if (!eglQuerySurface(display, surface, EGL_WIDTH, &surfaceWidth) ||
                !eglQuerySurface(display, surface, EGL_HEIGHT, &surfaceHeight))
            {
                Log(Log::Level::ERR) << "Failed to get query window size " <<  eglGetError();
                return false;
            }

            frameBufferWidth = surfaceWidth;
            frameBufferHeight = surfaceHeight;

            Size2 backBufferSize = Size2(static_cast<float>(frameBufferWidth),
                                         static_cast<float>(frameBufferHeight));

            windowAndroid->setSize(backBufferSize / windowAndroid->getContentScale());

            for (const std::unique_ptr<Resource>& resource : resources)
            {
                ResourceOGL* resourceOGL = reinterpret_cast<ResourceOGL*>(resource.get());
                if (!resourceOGL->reload())
                {
                    return false;
                }
            }

            if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
            {
                Log(Log::Level::ERR) << "Failed to unset EGL context";
            }

            running = true;
            renderThread = std::thread(&RenderDeviceOGLAndroid::main, this);

            return true;
        }

        bool RenderDeviceOGLAndroid::destroy()
        {
            running = false;
            flushCommands();
            if (renderThread.joinable()) renderThread.join();

            if (context)
            {
                if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
                {
                    Log(Log::Level::ERR) << "Failed to unset EGL context";
                }

                if (!eglDestroyContext(display, context))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL context";
                }

                context = nullptr;
            }

            if (surface)
            {
                if (!eglDestroySurface(display, surface))
                {
                    Log(Log::Level::ERR) << "Failed to destroy EGL surface";
                }

                surface = nullptr;
            }

            return true;
        }

        bool RenderDeviceOGLAndroid::lockContext()
        {
            if (!eglMakeCurrent(display, surface, surface, context))
            {
                Log(Log::Level::ERR) << "Failed to set current EGL context, error: " << eglGetError();
                return false;
            }

            return true;
        }

        bool RenderDeviceOGLAndroid::swapBuffers()
        {
            if (eglSwapBuffers(display, surface) != EGL_TRUE)
            {
                Log(Log::Level::ERR) << "Failed to swap buffers " << eglGetError();
                return false;
            }

            return true;
        }

        void RenderDeviceOGLAndroid::main()
        {
            while (running)
            {
                process();
            }

            if (!eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT))
            {
                Log(Log::Level::ERR) << "Failed to unset EGL context, error: " << eglGetError();
            }
        }
    } // namespace graphics
} // namespace ouzel

#endif
<|endoftext|>


<commit_before>#include "factory.hpp"
#include "Decerealiser.hpp"
#include <iostream>

namespace {
template<typename T, typename... A>
std::unique_ptr<T> make_unique(A... args) {
    return std::unique_ptr<T>(new T(args...));
}
} //anonymous namespace

std::unique_ptr<MqttMessage> MqttFactory::create(std::vector<ubyte> bytes) {
    Decerealiser cereal(bytes);
    auto fixedHeader = cereal.value<MqttFixedHeader>();
    // if(fixedHeader.remaining < cereal.getBytes().size()) {
    //     std::cerr << "Wrong MQTT remaining size " << (int)fixedHeader.remaining <<
    //         ". Real remaining size: " << cereal.getBytes().size() << std::endl;
    // }

    const auto mqttSize = fixedHeader.remaining + MqttFixedHeader::SIZE;
    if(mqttSize != bytes.size()) {
        std::cerr << "Malformed packet. Actual size: " << bytes.size() <<
            ". Advertised size: " << mqttSize <<
            " (r " << fixedHeader.remaining  << ")" << std::endl;
        return nullptr;
    }

    cereal.reset(); //so the messages created below can re-read the header

    switch(fixedHeader.type) {
    case MqttType::CONNECT:
        return cereal.create<MqttConnect>(fixedHeader);
    case MqttType::CONNACK:
        return cereal.create<MqttConnack>();
    case MqttType::PUBLISH:
        return cereal.create<MqttPublish>(fixedHeader);
    case MqttType::SUBSCRIBE:
        if(fixedHeader.qos != 1) {
            std::cerr << "SUBSCRIBE message with qos " << fixedHeader.qos <<  ", should be 1" << std::endl;
        }
        return cereal.create<MqttSubscribe>(fixedHeader);
    case MqttType::SUBACK:
        return cereal.create<MqttSuback>(fixedHeader);
    case MqttType::UNSUBSCRIBE:
        return cereal.create<MqttUnsubscribe>(fixedHeader);
    case MqttType::UNSUBACK:
        return cereal.create<MqttUnsuback>(fixedHeader);
    case MqttType::PINGREQ:
        return make_unique<MqttPingReq>();
    case MqttType::PINGRESP:
        return make_unique<MqttPingResp>();
    case MqttType::DISCONNECT:
        return make_unique<MqttDisconnect>();
    default:
        std::cerr << "Unknown MQTT message type: " << (int)fixedHeader.type << std::endl;
        return nullptr;
    }
}
<commit_msg>Fix usage of new cereal<commit_after>#include "factory.hpp"
#include "Decerealiser.hpp"
#include <iostream>

namespace {
template<typename T, typename... A>
std::unique_ptr<T> make_unique(A... args) {
    return std::unique_ptr<T>(new T(args...));
}
} //anonymous namespace

std::unique_ptr<MqttMessage> MqttFactory::create(std::vector<ubyte> bytes) {
    Decerealiser cereal(bytes);
    auto fixedHeader = cereal.value<MqttFixedHeader>();
    // if(fixedHeader.remaining < cereal.getBytes().size()) {
    //     std::cerr << "Wrong MQTT remaining size " << (int)fixedHeader.remaining <<
    //         ". Real remaining size: " << cereal.getBytes().size() << std::endl;
    // }

    const auto mqttSize = fixedHeader.remaining + MqttFixedHeader::SIZE;
    if(mqttSize != bytes.size()) {
        std::cerr << "Malformed packet. Actual size: " << bytes.size() <<
            ". Advertised size: " << mqttSize <<
            " (r " << fixedHeader.remaining  << ")" << std::endl;
        return nullptr;
    }

    cereal.reset(); //so the messages created below can re-read the header

    switch(fixedHeader.type) {
    case MqttType::CONNECT:
        return cereal.createPtr<MqttConnect>(fixedHeader);
    case MqttType::CONNACK:
        return cereal.createPtr<MqttConnack>();
    case MqttType::PUBLISH:
        return cereal.createPtr<MqttPublish>(fixedHeader);
    case MqttType::SUBSCRIBE:
        if(fixedHeader.qos != 1) {
            std::cerr << "SUBSCRIBE message with qos " << fixedHeader.qos <<  ", should be 1" << std::endl;
        }
        return cereal.createPtr<MqttSubscribe>(fixedHeader);
    case MqttType::SUBACK:
        return cereal.createPtr<MqttSuback>(fixedHeader);
    case MqttType::UNSUBSCRIBE:
        return cereal.createPtr<MqttUnsubscribe>(fixedHeader);
    case MqttType::UNSUBACK:
        return cereal.createPtr<MqttUnsuback>(fixedHeader);
    case MqttType::PINGREQ:
        return make_unique<MqttPingReq>();
    case MqttType::PINGRESP:
        return make_unique<MqttPingResp>();
    case MqttType::DISCONNECT:
        return make_unique<MqttDisconnect>();
    default:
        std::cerr << "Unknown MQTT message type: " << (int)fixedHeader.type << std::endl;
        return nullptr;
    }
}
<|endoftext|>


<commit_before>/**
 * Copyright (c) 2016-present, Facebook, Inc.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree. An additional grant
 * of patent rights can be found in the PATENTS file in the same directory.
 */

#include "matrix.h"
#include "vector.h"
#include "dictionary.h"
#include "model.h"
#include "utils.h"
#include "real.h"
#include "args.h"
#include <iostream>
#include <iomanip>
#include <thread>
#include <time.h>
#include <string>
#include <math.h>
#include <vector>
#include <atomic>
#include <algorithm>
#include <fenv.h>

Args args;

namespace info {
  clock_t start;
  std::atomic<int64_t> allWords(0);
  std::atomic<int64_t> allN(0);
  double allLoss(0.0);
}

void saveVectors(Dictionary& dict, Matrix& input, Matrix& output) {
  int32_t N = dict.getNumWords();
  std::ofstream ofs(args.output + ".vec");
  if (ofs.is_open()) {
    ofs << N << ' ' << args.dim << std::endl;
    for (int32_t i = 0; i < N; i++) {
      ofs << dict.getWord(i) << ' ';
      Vector vec(args.dim);
      vec.zero();
      const std::vector<int32_t>& ngrams = dict.getNgrams(i);
      for (auto it = ngrams.begin(); it != ngrams.end(); ++it) {
        vec.addRow(input, *it);
      }
      vec.mul(1.0 / ngrams.size());
      vec.writeToStream(ofs);
      ofs << std::endl;
    }
    ofs.close();
  } else {
    std::cout << "Error opening file for saving vectors." << std::endl;
  }
}

void printVectors(Dictionary& dict, Matrix& input, Matrix& output) {
  std::string ws;
  while (std::cin >> ws) {
    std::cout << ws << " ";
    Vector vec(args.dim);
    vec.zero();
    const std::vector<int32_t> ngrams = dict.getNgrams(ws);
    for (auto it = ngrams.begin(); it != ngrams.end(); ++it) {
      vec.addRow(input, *it);
    }
    vec.mul(1.0 / ngrams.size());
    vec.writeToStream(std::cout);
    std::cout << std::endl;
  }
}

void saveModel(Dictionary& dict, Matrix& input, Matrix& output) {
  std::ofstream ofs(args.output + ".bin");
  args.save(ofs);
  dict.save(ofs);
  input.save(ofs);
  output.save(ofs);
  ofs.close();
}

void loadModel(std::string filename, Dictionary& dict,
               Matrix& input, Matrix& output) {
  std::ifstream ifs(filename);
  args.load(ifs);
  dict.load(ifs);
  input.load(ifs);
  output.load(ifs);
  ifs.close();
}

void printInfo(Model& model, int64_t numTokens) {
  real progress = real(info::allWords) / (args.epoch * numTokens);
  real avLoss = info::allLoss / info::allN;
  float time = float(clock() - info::start) / CLOCKS_PER_SEC;
  float wst = float(info::allWords) / time;
  int eta = int(time / progress * (1 - progress) / args.thread);
  int etah = eta / 3600;
  int etam = (eta - etah * 3600) / 60;

  std::cout << std::fixed;
  std::cout << "\rProgress: " << std::setprecision(1) << 100 * progress << "%";
  std::cout << "  words/sec/thread: " << std::setprecision(0) << wst;
  std::cout << "  lr: " << std::setprecision(6) << model.getLearningRate();
  std::cout << "  loss: " << std::setprecision(6) << avLoss;
  std::cout << "  eta: " << etah << "h" << etam << "m  ";
  std::cout << std::flush;
}

void supervised(Model& model,
                const std::vector<int32_t>& line,
                const std::vector<int32_t>& labels,
                double& loss, int32_t& N) {
    if (labels.size() == 0 || line.size() == 0) return;
    std::uniform_int_distribution<> uniform(0, labels.size() - 1);
    int32_t i = uniform(model.rng);
    model.update(line, labels[i], loss, N);
}

void cbow(Dictionary& dict, Model& model,
          const std::vector<int32_t>& line,
          double& loss, int32_t& N) {
  int32_t n = line.size();
  std::vector<int32_t> bow;
  std::uniform_int_distribution<> uniform(1, args.ws);
  for (int32_t w = 0; w < n; w++) {
    int32_t wb = uniform(model.rng);
    bow.clear();
    for (int32_t c = -wb; c <= wb; c++) {
      if (c != 0 && w + c >= 0 && w + c < n) {
        const std::vector<int32_t>& ngrams = dict.getNgrams(line[w + c]);
        for (auto it = ngrams.cbegin(); it != ngrams.cend(); ++it) {
          bow.push_back(*it);
        }
      }
    }
    model.update(bow, line[w], loss, N);
  }
}

void skipGram(Dictionary& dict, Model& model,
              const std::vector<int32_t>& line,
              double& loss, int32_t& N) {
  int32_t n = line.size();
  std::uniform_int_distribution<> uniform(1, args.ws);
  for (int32_t w = 0; w < n; w++) {
    int32_t wb = uniform(model.rng);
    const std::vector<int32_t>& ngrams = dict.getNgrams(line[w]);
    for (int32_t c = -wb; c <= wb; c++) {
      if (c != 0 && w + c >= 0 && w + c < n) {
        int32_t target = line[w + c];
        model.update(ngrams, target, loss, N);
      }
    }
  }
}

void test(Dictionary& dict, Model& model, std::string filename) {
  int32_t nexamples = 0;
  double precision = 0.0;
  std::vector<int32_t> line, labels;
  std::ifstream ifs(filename);
  while (!ifs.eof()) {
    dict.getLine(ifs, line, labels, model.rng);
    dict.addNgrams(line, args.wordNgrams);
    if (labels.size() > 0 && line.size() > 0) {
      int32_t i = model.predict(line);
      if (std::find(labels.begin(), labels.end(), i) != labels.end()) {
        precision += 1.0;
      }
      nexamples++;
    }
  }
  ifs.close();
  std::cout << std::setprecision(3);
  std::cout << "P@1: " << precision / nexamples << std::endl;
  std::cout << "Number of examples: " << nexamples << std::endl;
}

void predict(Dictionary& dict, Model& model, std::string filename) {
  int32_t nexamples = 0;
  double precision = 0.0;
  std::vector<int32_t> line, labels;
  std::ifstream ifs(filename);
  while (!ifs.eof()) {
    dict.getLine(ifs, line, labels, model.rng);
    dict.addNgrams(line, args.wordNgrams);
    if (line.size() > 0) {
      int32_t i = model.predict(line);
      std::cout << dict.getLabel(i) << std::endl;
    } else {
      std::cout << "n/a" << std::endl;
    }
  }
  ifs.close();
}

void trainThread(Dictionary& dict, Matrix& input, Matrix& output,
                 int32_t threadId) {
  std::ifstream ifs(args.input);
  utils::seek(ifs, threadId * utils::size(ifs) / args.thread);

  Model model(input, output, args.dim, args.lr, threadId);
  if (args.model == model_name::sup) {
    model.setLabelFreq(dict.getLabelFreq());
  } else {
    model.setLabelFreq(dict.getWordFreq());
  }

  const int64_t ntokens = dict.getNumTokens();
  int64_t tokenCount = 0;
  double loss = 0.0;
  int32_t N = 0;
  std::vector<int32_t> line, labels;

  while (info::allWords < args.epoch * ntokens) {
    tokenCount += dict.getLine(ifs, line, labels, model.rng);
    if (args.model == model_name::sup) {
      dict.addNgrams(line, args.wordNgrams);
      supervised(model, line, labels, loss, N);
    } else if (args.model == model_name::cbow) {
      cbow(dict, model, line, loss, N);
    } else if (args.model == model_name::sg) {
      skipGram(dict, model, line, loss, N);
    }

    if (tokenCount > 10000) {
      info::allWords += tokenCount;
      info::allLoss += loss;
      info::allN += N;
      tokenCount = 0;
      loss = 0.0;
      N = 0;
      real progress = real(info::allWords) / (args.epoch * ntokens);
      model.setLearningRate(args.lr * (1.0 - progress));
      if (threadId == 0) printInfo(model, ntokens);
    }
  }
  if (threadId == 0) {
    printInfo(model, ntokens);
    std::cout << std::endl;
  }
  if (args.model == model_name::sup && threadId == 0) {
    test(dict, model, args.test);
  }
  ifs.close();
}

void printUsage() {
  std::cout
    << "usage: fasttext <command> <args>\n\n"
    << "The commands supported by fasttext are:\n\n"
    << "  supervised       train a supervised classifier\n"
    << "  test             evaluate a supervised classifier\n"
    << "  predict          predict most likely label\n"
    << "  skipgram         train a skipgram model\n"
    << "  cbow             train a cbow model\n"
    << "  print-vectors    print vectors given a trained model\n"
    << std::endl;
}

void printTestUsage() {
  std::cout
    << "usage: fasttext test <model> <test-data>\n\n"
    << "  <model>      model filename\n"
    << "  <test-data>  test data filename\n"
    << std::endl;
}

void printPredictUsage() {
  std::cout
    << "usage: fasttext predict <model> <test-data>\n\n"
    << "  <model>      model filename\n"
    << "  <test-data>  test data filename\n"
    << std::endl;
}

void printPrintVectorsUsage() {
  std::cout
    << "usage: fasttext print-vectors <model>\n\n"
    << "  <model>      model filename\n"
    << std::endl;
}

void test(int argc, char** argv) {
  if (argc != 4) {
    printTestUsage();
    exit(EXIT_FAILURE);
  }
  Dictionary dict;
  Matrix input, output;
  loadModel(std::string(argv[2]), dict, input, output);
  Model model(input, output, args.dim, args.lr, 1);
  model.setLabelFreq(dict.getLabelFreq());
  test(dict, model, std::string(argv[3]));
  exit(0);
}

void predict(int argc, char** argv) {
  if (argc != 4) {
    printPredictUsage();
    exit(EXIT_FAILURE);
  }
  Dictionary dict;
  Matrix input, output;
  loadModel(std::string(argv[2]), dict, input, output);
  Model model(input, output, args.dim, args.lr, 1);
  model.setLabelFreq(dict.getLabelFreq());
  predict(dict, model, std::string(argv[3]));
  exit(0);
}

void printVectors(int argc, char** argv) {
  if (argc != 3) {
    printPrintVectorsUsage();
    exit(EXIT_FAILURE);
  }
  Dictionary dict;
  Matrix input, output;
  loadModel(std::string(argv[2]), dict, input, output);
  printVectors(dict, input, output);
  exit(0);
}

void train(int argc, char** argv) {
  args.parseArgs(argc, argv);

  Dictionary dict;
  dict.readFromFile(args.input);
  Matrix input(dict.getNumWords() + args.bucket, args.dim);
  Matrix output;
  if (args.model == model_name::sup) {
    output = Matrix(dict.getNumLabels(), args.dim);
  } else {
    output = Matrix(dict.getNumWords(), args.dim);
  }
  input.uniform(1.0 / args.dim);
  output.zero();

  info::start = clock();
  std::vector<std::thread> threads;
  for (int32_t i = 0; i < args.thread; i++) {
    threads.push_back(std::thread(&trainThread, std::ref(dict),
                                  std::ref(input), std::ref(output), i));
  }
  for (auto it = threads.begin(); it != threads.end(); ++it) {
    it->join();
  }
  float trainTime = float(clock() - info::start) / CLOCKS_PER_SEC / args.thread;
  std::cout << "training took: " << trainTime << " sec" << std::endl;

  if (args.output.size() != 0) {
    saveModel(dict, input, output);
    saveVectors(dict, input, output);
  }
}

int main(int argc, char** argv) {
  std::locale::global(std::locale(""));
  utils::initTables();
  if (argc < 2) {
    printUsage();
    exit(EXIT_FAILURE);
  }
  std::string command(argv[1]);
  if (command == "skipgram" || command == "cbow" || command == "supervised") {
    train(argc, argv);
  } else if (command == "test") {
    test(argc, argv);
  } else if (command == "print-vectors") {
    printVectors(argc, argv);
  } else if (command == "predict") {
    predict(argc, argv);
  } else {
    printUsage();
    exit(EXIT_FAILURE);
  }
  utils::freeTables();
  return 0;
}
<commit_msg>added a wall clock timing of training<commit_after>/**
 * Copyright (c) 2016-present, Facebook, Inc.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree. An additional grant
 * of patent rights can be found in the PATENTS file in the same directory.
 */

#include "matrix.h"
#include "vector.h"
#include "dictionary.h"
#include "model.h"
#include "utils.h"
#include "real.h"
#include "args.h"
#include <iostream>
#include <iomanip>
#include <thread>
#include <time.h>
#include <string>
#include <math.h>
#include <vector>
#include <atomic>
#include <algorithm>
#include <fenv.h>

Args args;

namespace info {
  time_t startTime;
  time_t nowTime;
  clock_t start;
  std::atomic<int64_t> allWords(0);
  std::atomic<int64_t> allN(0);
  double allLoss(0.0);
}

void saveVectors(Dictionary& dict, Matrix& input, Matrix& output) {
  int32_t N = dict.getNumWords();
  std::ofstream ofs(args.output + ".vec");
  if (ofs.is_open()) {
    ofs << N << ' ' << args.dim << std::endl;
    for (int32_t i = 0; i < N; i++) {
      ofs << dict.getWord(i) << ' ';
      Vector vec(args.dim);
      vec.zero();
      const std::vector<int32_t>& ngrams = dict.getNgrams(i);
      for (auto it = ngrams.begin(); it != ngrams.end(); ++it) {
        vec.addRow(input, *it);
      }
      vec.mul(1.0 / ngrams.size());
      vec.writeToStream(ofs);
      ofs << std::endl;
    }
    ofs.close();
  } else {
    std::cout << "Error opening file for saving vectors." << std::endl;
  }
}

void printVectors(Dictionary& dict, Matrix& input, Matrix& output) {
  std::string ws;
  while (std::cin >> ws) {
    std::cout << ws << " ";
    Vector vec(args.dim);
    vec.zero();
    const std::vector<int32_t> ngrams = dict.getNgrams(ws);
    for (auto it = ngrams.begin(); it != ngrams.end(); ++it) {
      vec.addRow(input, *it);
    }
    vec.mul(1.0 / ngrams.size());
    vec.writeToStream(std::cout);
    std::cout << std::endl;
  }
}

void saveModel(Dictionary& dict, Matrix& input, Matrix& output) {
  std::ofstream ofs(args.output + ".bin");
  args.save(ofs);
  dict.save(ofs);
  input.save(ofs);
  output.save(ofs);
  ofs.close();
}

void loadModel(std::string filename, Dictionary& dict,
               Matrix& input, Matrix& output) {
  std::ifstream ifs(filename);
  args.load(ifs);
  dict.load(ifs);
  input.load(ifs);
  output.load(ifs);
  ifs.close();
}

void printInfo(Model& model, int64_t numTokens) {
  real progress = real(info::allWords) / (args.epoch * numTokens);
  real avLoss = info::allLoss / info::allN;
  float time = float(clock() - info::start) / CLOCKS_PER_SEC;
  float wst = float(info::allWords) / time;
  int eta = int(time / progress * (1 - progress) / args.thread);
  int etah = eta / 3600;
  int etam = (eta - etah * 3600) / 60;

  std::cout << std::fixed;
  std::cout << "\rProgress: " << std::setprecision(1) << 100 * progress << "%";
  std::cout << "  words/sec/thread: " << std::setprecision(0) << wst;
  std::cout << "  lr: " << std::setprecision(6) << model.getLearningRate();
  std::cout << "  loss: " << std::setprecision(6) << avLoss;
  std::cout << "  eta: " << etah << "h" << etam << "m  ";
  std::cout << std::flush;
}

void supervised(Model& model,
                const std::vector<int32_t>& line,
                const std::vector<int32_t>& labels,
                double& loss, int32_t& N) {
    if (labels.size() == 0 || line.size() == 0) return;
    std::uniform_int_distribution<> uniform(0, labels.size() - 1);
    int32_t i = uniform(model.rng);
    model.update(line, labels[i], loss, N);
}

void cbow(Dictionary& dict, Model& model,
          const std::vector<int32_t>& line,
          double& loss, int32_t& N) {
  int32_t n = line.size();
  std::vector<int32_t> bow;
  std::uniform_int_distribution<> uniform(1, args.ws);
  for (int32_t w = 0; w < n; w++) {
    int32_t wb = uniform(model.rng);
    bow.clear();
    for (int32_t c = -wb; c <= wb; c++) {
      if (c != 0 && w + c >= 0 && w + c < n) {
        const std::vector<int32_t>& ngrams = dict.getNgrams(line[w + c]);
        for (auto it = ngrams.cbegin(); it != ngrams.cend(); ++it) {
          bow.push_back(*it);
        }
      }
    }
    model.update(bow, line[w], loss, N);
  }
}

void skipGram(Dictionary& dict, Model& model,
              const std::vector<int32_t>& line,
              double& loss, int32_t& N) {
  int32_t n = line.size();
  std::uniform_int_distribution<> uniform(1, args.ws);
  for (int32_t w = 0; w < n; w++) {
    int32_t wb = uniform(model.rng);
    const std::vector<int32_t>& ngrams = dict.getNgrams(line[w]);
    for (int32_t c = -wb; c <= wb; c++) {
      if (c != 0 && w + c >= 0 && w + c < n) {
        int32_t target = line[w + c];
        model.update(ngrams, target, loss, N);
      }
    }
  }
}

void test(Dictionary& dict, Model& model, std::string filename) {
  int32_t nexamples = 0;
  double precision = 0.0;
  std::vector<int32_t> line, labels;
  std::ifstream ifs(filename);
  while (!ifs.eof()) {
    dict.getLine(ifs, line, labels, model.rng);
    dict.addNgrams(line, args.wordNgrams);
    if (labels.size() > 0 && line.size() > 0) {
      int32_t i = model.predict(line);
      if (std::find(labels.begin(), labels.end(), i) != labels.end()) {
        precision += 1.0;
      }
      nexamples++;
    }
  }
  ifs.close();
  std::cout << std::setprecision(3);
  std::cout << "P@1: " << precision / nexamples << std::endl;
  std::cout << "Number of examples: " << nexamples << std::endl;
}

void predict(Dictionary& dict, Model& model, std::string filename) {
  int32_t nexamples = 0;
  double precision = 0.0;
  std::vector<int32_t> line, labels;
  std::ifstream ifs(filename);
  while (!ifs.eof()) {
    dict.getLine(ifs, line, labels, model.rng);
    dict.addNgrams(line, args.wordNgrams);
    if (line.size() > 0) {
      int32_t i = model.predict(line);
      std::cout << dict.getLabel(i) << std::endl;
    } else {
      std::cout << "n/a" << std::endl;
    }
  }
  ifs.close();
}

void trainThread(Dictionary& dict, Matrix& input, Matrix& output,
                 int32_t threadId) {
  std::ifstream ifs(args.input);
  utils::seek(ifs, threadId * utils::size(ifs) / args.thread);

  Model model(input, output, args.dim, args.lr, threadId);
  if (args.model == model_name::sup) {
    model.setLabelFreq(dict.getLabelFreq());
  } else {
    model.setLabelFreq(dict.getWordFreq());
  }

  const int64_t ntokens = dict.getNumTokens();
  int64_t tokenCount = 0;
  double loss = 0.0;
  int32_t N = 0;
  std::vector<int32_t> line, labels;

  while (info::allWords < args.epoch * ntokens) {
    tokenCount += dict.getLine(ifs, line, labels, model.rng);
    if (args.model == model_name::sup) {
      dict.addNgrams(line, args.wordNgrams);
      supervised(model, line, labels, loss, N);
    } else if (args.model == model_name::cbow) {
      cbow(dict, model, line, loss, N);
    } else if (args.model == model_name::sg) {
      skipGram(dict, model, line, loss, N);
    }

    if (tokenCount > 10000) {
      info::allWords += tokenCount;
      info::allLoss += loss;
      info::allN += N;
      tokenCount = 0;
      loss = 0.0;
      N = 0;
      real progress = real(info::allWords) / (args.epoch * ntokens);
      model.setLearningRate(args.lr * (1.0 - progress));
      if (threadId == 0) printInfo(model, ntokens);
    }
  }
  if (threadId == 0) {
    printInfo(model, ntokens);
    std::cout << std::endl;
  }
  if (args.model == model_name::sup && threadId == 0) {
    test(dict, model, args.test);
  }
  ifs.close();
}

void printUsage() {
  std::cout
    << "usage: fasttext <command> <args>\n\n"
    << "The commands supported by fasttext are:\n\n"
    << "  supervised       train a supervised classifier\n"
    << "  test             evaluate a supervised classifier\n"
    << "  predict          predict most likely label\n"
    << "  skipgram         train a skipgram model\n"
    << "  cbow             train a cbow model\n"
    << "  print-vectors    print vectors given a trained model\n"
    << std::endl;
}

void printTestUsage() {
  std::cout
    << "usage: fasttext test <model> <test-data>\n\n"
    << "  <model>      model filename\n"
    << "  <test-data>  test data filename\n"
    << std::endl;
}

void printPredictUsage() {
  std::cout
    << "usage: fasttext predict <model> <test-data>\n\n"
    << "  <model>      model filename\n"
    << "  <test-data>  test data filename\n"
    << std::endl;
}

void printPrintVectorsUsage() {
  std::cout
    << "usage: fasttext print-vectors <model>\n\n"
    << "  <model>      model filename\n"
    << std::endl;
}

void test(int argc, char** argv) {
  if (argc != 4) {
    printTestUsage();
    exit(EXIT_FAILURE);
  }
  Dictionary dict;
  Matrix input, output;
  loadModel(std::string(argv[2]), dict, input, output);
  Model model(input, output, args.dim, args.lr, 1);
  model.setLabelFreq(dict.getLabelFreq());
  test(dict, model, std::string(argv[3]));
  exit(0);
}

void predict(int argc, char** argv) {
  if (argc != 4) {
    printPredictUsage();
    exit(EXIT_FAILURE);
  }
  Dictionary dict;
  Matrix input, output;
  loadModel(std::string(argv[2]), dict, input, output);
  Model model(input, output, args.dim, args.lr, 1);
  model.setLabelFreq(dict.getLabelFreq());
  predict(dict, model, std::string(argv[3]));
  exit(0);
}

void printVectors(int argc, char** argv) {
  if (argc != 3) {
    printPrintVectorsUsage();
    exit(EXIT_FAILURE);
  }
  Dictionary dict;
  Matrix input, output;
  loadModel(std::string(argv[2]), dict, input, output);
  printVectors(dict, input, output);
  exit(0);
}

void train(int argc, char** argv) {
  args.parseArgs(argc, argv);

  Dictionary dict;
  dict.readFromFile(args.input);
  Matrix input(dict.getNumWords() + args.bucket, args.dim);
  Matrix output;
  if (args.model == model_name::sup) {
    output = Matrix(dict.getNumLabels(), args.dim);
  } else {
    output = Matrix(dict.getNumWords(), args.dim);
  }
  input.uniform(1.0 / args.dim);
  output.zero();

  info::start = clock();
  time(&info::startTime);
  std::vector<std::thread> threads;
  for (int32_t i = 0; i < args.thread; i++) {
    threads.push_back(std::thread(&trainThread, std::ref(dict),
                                  std::ref(input), std::ref(output), i));
  }
  for (auto it = threads.begin(); it != threads.end(); ++it) {
    it->join();
  }
  time(&info::nowTime);
  double trainTime = difftime(info::nowTime, info::startTime);
  std::cout << "training took: " << trainTime << " sec" << std::endl;

  if (args.output.size() != 0) {
    saveModel(dict, input, output);
    saveVectors(dict, input, output);
  }
}

int main(int argc, char** argv) {
  std::locale::global(std::locale(""));
  utils::initTables();
  if (argc < 2) {
    printUsage();
    exit(EXIT_FAILURE);
  }
  std::string command(argv[1]);
  if (command == "skipgram" || command == "cbow" || command == "supervised") {
    train(argc, argv);
  } else if (command == "test") {
    test(argc, argv);
  } else if (command == "print-vectors") {
    printVectors(argc, argv);
  } else if (command == "predict") {
    predict(argc, argv);
  } else {
    printUsage();
    exit(EXIT_FAILURE);
  }
  utils::freeTables();
  return 0;
}
<|endoftext|>


<commit_before>#include <ctime>
#include <cctype>

#include "core/RoleFactory.h"
#include "config/constraints.h"
#include "EventLogger.h"
#include "util/EventSender.h"

#include "msgpack_decode.h"

static RoleConfigGroup el_config("eventlogger");
static ConfigVariable<std::string> bind_addr("bind", "0.0.0.0:7197", el_config);
static ConfigVariable<std::string> output_format("output", "events-%Y%m%d-%H%M%S.csv", el_config);
static ConfigVariable<std::string> rotate_interval("rotate_interval", "0", el_config);
static ValidAddressConstraint valid_bind_addr(bind_addr);

EventLogger::EventLogger(RoleConfig roleconfig) : Role(roleconfig),
    m_log("eventlogger", "Event Logger"), m_file(0)
{
    bind(bind_addr.get_rval(roleconfig));

    m_file_format = output_format.get_rval(roleconfig);
    open_log();

    LoggedEvent event("log-opened", "EventLogger");
    event.add("msg", "Log opened upon Event Logger startup.");
    process_packet(event.make_datagram());

    start_receive();
}

void EventLogger::bind(const std::string &addr)
{
    m_log.info() << "Opening UDP socket..." << std::endl;
    boost::system::error_code ec;
    auto addresses = resolve_address(addr, 7197, io_service, ec);
    if(ec.value() != 0) {
        m_log.fatal() << "Couldn't resolve " << addr << std::endl;
        exit(1);
    }

    m_socket = new udp::socket(io_service,
                               udp::endpoint(addresses[0].address(), addresses[0].port()));
}

void EventLogger::open_log()
{
    time_t rawtime;
    time(&rawtime);

    char filename[1024];
    strftime(filename, 1024, m_file_format.c_str(), localtime(&rawtime));
    m_log.debug() << "New log filename: " << filename << std::endl;

    if(m_file) {
        m_file->close();
    }

    m_file = new std::ofstream(filename);

    m_log.info() << "Opened new log." << std::endl;
}

void EventLogger::cycle_log()
{
    open_log();

    LoggedEvent event("log-opened", "EventLogger");
    event.add("msg", "Log cycled.");
    process_packet(event.make_datagram());
}

void EventLogger::process_packet(DatagramHandle dg)
{
    DatagramIterator dgi(dg);
    std::stringstream stream;

    try {
        msgpack_decode(stream, dgi);
    } catch(DatagramIteratorEOF&) {
        m_log.error() << "Received truncated packet from "
                      << m_remote.address() << ":" << m_remote.port() << std::endl;
        return;
    }

    if(dgi.tell() != dg->size()) {
        m_log.error() << "Received packet with extraneous data from "
                      << m_remote.address() << ":" << m_remote.port() << std::endl;
        return;
    }

    std::string data = stream.str();
    m_log.trace() << "Received: " << data << std::endl;

    // This is a little bit of a kludge, but we should make sure we got a
    // MessagePack map as the event log element, and not some other type. The
    // easiest way to do this is to make sure that the JSON representation
    // begins with {
    if(data[0] != '{') {
        m_log.error() << "Received non-map event log from "
                      << m_remote.address() << ":" << m_remote.port()
                      << ": " << data << std::endl;
        return;
    }

    // Now let's insert our timestamp:
    time_t rawtime;
    time(&rawtime);

    char timestamp[64];
    strftime(timestamp, 64, "{\"_time\": \"%Y-%m-%d %H:%M:%S%z\", ", localtime(&rawtime));

    *m_file << timestamp << data.substr(1) << std::endl;
}

void EventLogger::start_receive()
{
    m_socket->async_receive_from(boost::asio::buffer(m_buffer, EVENTLOG_BUFSIZE),
                                 m_remote, boost::bind(&EventLogger::handle_receive, this,
                                         boost::asio::placeholders::error,
                                         boost::asio::placeholders::bytes_transferred));
}

void EventLogger::handle_receive(const boost::system::error_code &ec, std::size_t bytes)
{
    if(ec.value()) {
        m_log.warning() << "While receiving packet from "
                        << m_remote.address() << ":" << m_remote.port()
                        << ", an error occurred: "
                        << ec.value() << std::endl;
        return;
    }

    m_log.trace() << "Got packet from "
                  << m_remote.address() << ":" << m_remote.port() << std::endl;

    DatagramPtr dg = Datagram::create(m_buffer, bytes);
    process_packet(dg);

    start_receive();
}

static RoleFactoryItem<EventLogger> el_fact("eventlogger");
<commit_msg>EventLogger: Change default file extension to .log, not .csv.<commit_after>#include <ctime>
#include <cctype>

#include "core/RoleFactory.h"
#include "config/constraints.h"
#include "EventLogger.h"
#include "util/EventSender.h"

#include "msgpack_decode.h"

static RoleConfigGroup el_config("eventlogger");
static ConfigVariable<std::string> bind_addr("bind", "0.0.0.0:7197", el_config);
static ConfigVariable<std::string> output_format("output", "events-%Y%m%d-%H%M%S.log", el_config);
static ConfigVariable<std::string> rotate_interval("rotate_interval", "0", el_config);
static ValidAddressConstraint valid_bind_addr(bind_addr);

EventLogger::EventLogger(RoleConfig roleconfig) : Role(roleconfig),
    m_log("eventlogger", "Event Logger"), m_file(0)
{
    bind(bind_addr.get_rval(roleconfig));

    m_file_format = output_format.get_rval(roleconfig);
    open_log();

    LoggedEvent event("log-opened", "EventLogger");
    event.add("msg", "Log opened upon Event Logger startup.");
    process_packet(event.make_datagram());

    start_receive();
}

void EventLogger::bind(const std::string &addr)
{
    m_log.info() << "Opening UDP socket..." << std::endl;
    boost::system::error_code ec;
    auto addresses = resolve_address(addr, 7197, io_service, ec);
    if(ec.value() != 0) {
        m_log.fatal() << "Couldn't resolve " << addr << std::endl;
        exit(1);
    }

    m_socket = new udp::socket(io_service,
                               udp::endpoint(addresses[0].address(), addresses[0].port()));
}

void EventLogger::open_log()
{
    time_t rawtime;
    time(&rawtime);

    char filename[1024];
    strftime(filename, 1024, m_file_format.c_str(), localtime(&rawtime));
    m_log.debug() << "New log filename: " << filename << std::endl;

    if(m_file) {
        m_file->close();
    }

    m_file = new std::ofstream(filename);

    m_log.info() << "Opened new log." << std::endl;
}

void EventLogger::cycle_log()
{
    open_log();

    LoggedEvent event("log-opened", "EventLogger");
    event.add("msg", "Log cycled.");
    process_packet(event.make_datagram());
}

void EventLogger::process_packet(DatagramHandle dg)
{
    DatagramIterator dgi(dg);
    std::stringstream stream;

    try {
        msgpack_decode(stream, dgi);
    } catch(DatagramIteratorEOF&) {
        m_log.error() << "Received truncated packet from "
                      << m_remote.address() << ":" << m_remote.port() << std::endl;
        return;
    }

    if(dgi.tell() != dg->size()) {
        m_log.error() << "Received packet with extraneous data from "
                      << m_remote.address() << ":" << m_remote.port() << std::endl;
        return;
    }

    std::string data = stream.str();
    m_log.trace() << "Received: " << data << std::endl;

    // This is a little bit of a kludge, but we should make sure we got a
    // MessagePack map as the event log element, and not some other type. The
    // easiest way to do this is to make sure that the JSON representation
    // begins with {
    if(data[0] != '{') {
        m_log.error() << "Received non-map event log from "
                      << m_remote.address() << ":" << m_remote.port()
                      << ": " << data << std::endl;
        return;
    }

    // Now let's insert our timestamp:
    time_t rawtime;
    time(&rawtime);

    char timestamp[64];
    strftime(timestamp, 64, "{\"_time\": \"%Y-%m-%d %H:%M:%S%z\", ", localtime(&rawtime));

    *m_file << timestamp << data.substr(1) << std::endl;
}

void EventLogger::start_receive()
{
    m_socket->async_receive_from(boost::asio::buffer(m_buffer, EVENTLOG_BUFSIZE),
                                 m_remote, boost::bind(&EventLogger::handle_receive, this,
                                         boost::asio::placeholders::error,
                                         boost::asio::placeholders::bytes_transferred));
}

void EventLogger::handle_receive(const boost::system::error_code &ec, std::size_t bytes)
{
    if(ec.value()) {
        m_log.warning() << "While receiving packet from "
                        << m_remote.address() << ":" << m_remote.port()
                        << ", an error occurred: "
                        << ec.value() << std::endl;
        return;
    }

    m_log.trace() << "Got packet from "
                  << m_remote.address() << ":" << m_remote.port() << std::endl;

    DatagramPtr dg = Datagram::create(m_buffer, bytes);
    process_packet(dg);

    start_receive();
}

static RoleFactoryItem<EventLogger> el_fact("eventlogger");
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include <spatiotemporalexploration/ExecutionAction.h>
#include <strands_navigation_msgs/MonitoredNavigationAction.h>

#include "spatiotemporalexploration/AddView.h"
#include "spatiotemporalexploration/Visualize.h"

#include <dynamic_reconfigure/DoubleParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
#include <dynamic_reconfigure/Config.h>

using namespace std;

int ptuMovementFinished = 0;

bool drawEmptyCells = false;
bool drawCells = true;
bool saveFlag = false;

ros::Publisher ptu_pub;
sensor_msgs::JointState ptu;
spatiotemporalexploration::AddView measure_srv;
spatiotemporalexploration::Visualize visualize_srv;

ros::ServiceClient *measure_client_ptr, *visualize_client_ptr;

typedef actionlib::SimpleActionServer<spatiotemporalexploration::ExecutionAction> Server;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> *ac_nav_ptr;

void movePtu(float pan,float tilt)
{
    ptuMovementFinished = 0;
    ptu.name[0] ="pan";
    ptu.name[1] ="tilt";
    ptu.position[0] = pan;
    ptu.position[1] = tilt;
    ptu.velocity[0] = ptu.velocity[1] = 1.0;
    ptu_pub.publish(ptu);
}

void ptuCallback(const sensor_msgs::JointState::ConstPtr &msg)
{
    float pan,tilt;
    for (int i = 0;i<2;i++){
        if (msg->name[i] == "pan") pan = msg->position[i];
        if (msg->name[i] == "tilt") tilt = msg->position[i];
    }
    //printf("PTU: %.3f %.3f %i\n",pan,tilt,ptuMovementFinished);
    if (fabs(pan-ptu.position[0])<0.01 && fabs(tilt-ptu.position[1])<0.01) ptuMovementFinished++;
}

void execute(const spatiotemporalexploration::ExecutionGoalConstPtr& goal, Server* as)
{

    ROS_INFO("received new plan");
    as->acceptNewGoal();
    //as->setPreempted();

    strands_navigation_msgs::MonitoredNavigationGoal current_goal;

    spatiotemporalexploration::ExecutionFeedback feedback;

    sleep(1);

    int numPoints = 14;
    int point = 0;
    float pan[] =  { 0.00, 0.90, 1.80, 2.70, 2.70, 1.80, 0.90, 0.00,-0.90,-1.80,-2.70,-2.70,-1.80,-0.90,0.00};
    float tilt[] = { 0.50, 0.50, 0.50, 0.50,-0.20,-0.20,-0.20,-0.20,-0.20,-0.20,-0.20, 0.50, 0.50, 0.50,0.00};

    int n = (int) goal->locations.poses.size();

    ROS_INFO("received %d locations to visit in %f minutes", n, 0.0);

    ROS_INFO("undocking...");

    //Undocking
    current_goal.action_server = "undocking";
    current_goal.target_pose.header.frame_id = "map";
    ac_nav_ptr->sendGoal(current_goal);
    ac_nav_ptr->waitForResult(ros::Duration(0.0));

    current_goal.action_server = "move_base";
    current_goal.target_pose.header.frame_id = "map";

    if (ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("undocking sucessful!!! I'm going places!");
        for(int i = 0; i < n; i++)
        {
            char cr_goal[10];
            sprintf(cr_goal, "%d/%d", i, n);
            ROS_INFO("Location: %d/%d", i, n);
            feedback.current_goal = cr_goal;
            feedback.time_remaining = 0;//TODO
            as->publishFeedback(feedback);

            current_goal.target_pose.pose = goal->locations.poses[i];
            ROS_INFO("moving to location %d -> (%f,%f)",  i, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
            ac_nav_ptr->sendGoal(current_goal);

    //        ROS_INFO("moving to location %d -> (%f,%f)",  i, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
            ac_nav_ptr->waitForResult(ros::Duration(0.0));
	    if (i>0 && i < n-1){
		    if (ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
		    {
			    ROS_INFO("Monitored navigation: SUCCEEDED!");
			    point = 0;
			    movePtu(pan[point],tilt[point]);

			    ros::spinOnce();
			    while (ros::ok() && point < numPoints)
			    {
				    measure_srv.request.stamp = 0.0;
				    if (ptuMovementFinished > 10)
				    {
					    if(measure_client_ptr->call(measure_srv))
					    {
						    ROS_INFO("Measure added to grid!");
					    }
					    else
					    {
						    ROS_ERROR("Failed to call measure service");
						    exit(1);
					    }

					    point++;
					    movePtu(pan[point],tilt[point]);
					    ros::spinOnce();
					    usleep(500000);
					    if(drawCells){
						    visualize_srv.request.red = visualize_srv.request.blue = 0.0;
						    visualize_srv.request.green = visualize_srv.request.alpha = 1.0;
						    visualize_srv.request.minProbability = 0.9;
						    visualize_srv.request.maxProbability = 1.0;
						    visualize_srv.request.name = "occupied";
						    visualize_srv.request.type = 0;
						    visualize_client_ptr->call(visualize_srv);
						    ros::spinOnce();
						    usleep(100000);
						    if (drawEmptyCells){
							    visualize_srv.request.green = 0.0;
							    visualize_srv.request.red = 1.0;
							    visualize_srv.request.minProbability = 0.0;
							    visualize_srv.request.maxProbability = 0.1;
							    visualize_srv.request.alpha = 0.005;
							    visualize_srv.request.name = "free";
							    visualize_srv.request.type = 0;
							    visualize_client_ptr->call(visualize_srv);
							    ros::spinOnce();
							    usleep(100000);
						    }
					    }
				    }
				    ros::spinOnce();
			    }
		    }
		    else if(ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::PREEMPTED || ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::ABORTED)
		    {
			    ROS_INFO("Move base failed! Scanning this area...");
			    point = 0;
			    movePtu(pan[point],tilt[point]);

			    ros::spinOnce();
			    while (ros::ok() && point < numPoints)
			    {
				    measure_srv.request.stamp = 0.0;
				    if (ptuMovementFinished > 10)
				    {
					    if(measure_client_ptr->call(measure_srv))
					    {
						    ROS_INFO("Measure added to grid!");
					    }
					    else
					    {
						    ROS_ERROR("Failed to call measure service");
						    exit(1);
					    }

					    point++;
					    movePtu(pan[point],tilt[point]);
					    ros::spinOnce();
					    usleep(500000);
					    if(drawCells){
						    visualize_srv.request.red = visualize_srv.request.blue = 0.0;
						    visualize_srv.request.green = visualize_srv.request.alpha = 1.0;
						    visualize_srv.request.minProbability = 0.9;
						    visualize_srv.request.maxProbability = 1.0;
						    visualize_srv.request.name = "occupied";
						    visualize_srv.request.type = 0;
						    visualize_client_ptr->call(visualize_srv);
						    ros::spinOnce();
						    usleep(100000);
						    if (drawEmptyCells){
							    visualize_srv.request.green = 0.0;
							    visualize_srv.request.red = 1.0;
							    visualize_srv.request.minProbability = 0.0;
							    visualize_srv.request.maxProbability = 0.1;
							    visualize_srv.request.alpha = 0.005;
							    visualize_srv.request.name = "free";
							    visualize_srv.request.type = 0;
							    visualize_client_ptr->call(visualize_srv);
							    ros::spinOnce();
							    usleep(100000);
						    }
					    }
				    }
				    ros::spinOnce();
			    }
			    ROS_INFO("Moving to the next goal!");
		    }
	    }
            movePtu(0.0,0.0);
        }

        ROS_INFO("my work is done! going to charging station...");
        //current_goal.target_pose.pose.position.x = -0.4;
        //current_goal.target_pose.pose.position.y = 0.0;
        //current_goal.target_pose.pose.orientation.w = 1.0;
        //ac_nav_ptr->sendGoal(current_goal);
        //ac_nav_ptr->waitForResult(ros::Duration(0.0));

        //Docking
        current_goal.action_server = "docking";
        current_goal.target_pose.header.frame_id = "map";
        ac_nav_ptr->sendGoal(current_goal);
        ac_nav_ptr->waitForResult(ros::Duration(0.0));
    }
    else{
        ROS_INFO("undocking failed!");
    }

    as->setSucceeded();

}

int main(int argc,char *argv[])
{
    ros::init(argc, argv, "exploration_executioner");
    ros::NodeHandle n;

    ros::NodeHandle nh("~");

    Server server(n, "executioner", boost::bind(&execute, _1, &server), false);
    ROS_INFO("starting server...");
    server.start();

    actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> ac_nav("monitored_navigation",true);
    ac_nav_ptr = &ac_nav;
    ac_nav.waitForServer();

    //Subscribers
    ros::Subscriber ptu_sub = n.subscribe("/ptu/state", 10, ptuCallback);
    ptu.name.resize(3);
    ptu.position.resize(3);
    ptu.velocity.resize(3);
    ptu_pub = n.advertise<sensor_msgs::JointState>("/ptu/cmd", 10);

    //measure service client
    ros::ServiceClient measure_client = n.serviceClient<spatiotemporalexploration::AddView>("/fremenGrid/depth");
    measure_client_ptr = &measure_client;

    //vizualize client
    ros::ServiceClient visualize_client = n.serviceClient<spatiotemporalexploration::Visualize>("/fremenGrid/visualize");
    visualize_client_ptr = &visualize_client;

    //Dynamic Reconfigure
    dynamic_reconfigure::ReconfigureRequest srv_req;
    dynamic_reconfigure::ReconfigureResponse srv_resp;
    dynamic_reconfigure::DoubleParameter double_param;
    dynamic_reconfigure::Config conf;

    double_param.name = "yaw_goal_tolerance";
    double_param.value = 3.0;
    conf.doubles.push_back(double_param);

    srv_req.config = conf;

    ros::service::call("/move_base/DWAPlannerROS/set_parameters", srv_req, srv_resp);

    ROS_INFO("server started");

    ros::spin();

    return 0;
}
<commit_msg>improved navigation<commit_after>#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseArray.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/server/simple_action_server.h>
#include <actionlib/client/simple_action_client.h>

#include <spatiotemporalexploration/ExecutionAction.h>
#include <strands_navigation_msgs/MonitoredNavigationAction.h>

#include "spatiotemporalexploration/AddView.h"
#include "spatiotemporalexploration/Visualize.h"

#include <dynamic_reconfigure/DoubleParameter.h>
#include <dynamic_reconfigure/Reconfigure.h>
#include <dynamic_reconfigure/Config.h>

using namespace std;

int ptuMovementFinished = 0;

bool drawEmptyCells = false;
bool drawCells = true;
bool saveFlag = false;

ros::Publisher ptu_pub;
sensor_msgs::JointState ptu;
spatiotemporalexploration::AddView measure_srv;
spatiotemporalexploration::Visualize visualize_srv;

ros::ServiceClient *measure_client_ptr, *visualize_client_ptr;

typedef actionlib::SimpleActionServer<spatiotemporalexploration::ExecutionAction> Server;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> *ac_nav_ptr;

void movePtu(float pan,float tilt)
{
    ptuMovementFinished = 0;
    ptu.name[0] ="pan";
    ptu.name[1] ="tilt";
    ptu.position[0] = pan;
    ptu.position[1] = tilt;
    ptu.velocity[0] = ptu.velocity[1] = 1.0;
    ptu_pub.publish(ptu);
}

void ptuCallback(const sensor_msgs::JointState::ConstPtr &msg)
{
    float pan,tilt;
    for (int i = 0;i<2;i++){
        if (msg->name[i] == "pan") pan = msg->position[i];
        if (msg->name[i] == "tilt") tilt = msg->position[i];
    }
    //printf("PTU: %.3f %.3f %i\n",pan,tilt,ptuMovementFinished);
    if (fabs(pan-ptu.position[0])<0.01 && fabs(tilt-ptu.position[1])<0.01) ptuMovementFinished++;
}

void execute(const spatiotemporalexploration::ExecutionGoalConstPtr& goal, Server* as)
{

    ROS_INFO("received new plan");
    as->acceptNewGoal();
    //as->setPreempted();

    strands_navigation_msgs::MonitoredNavigationGoal current_goal;
    spatiotemporalexploration::ExecutionFeedback feedback;

    sleep(1);

    int numPoints = 14;
    int point = 0;
    float pan[] =  { 0.00, 0.90, 1.80, 2.70, 2.70, 1.80, 0.90, 0.00,-0.90,-1.80,-2.70,-2.70,-1.80,-0.90,0.00};
    float tilt[] = { 0.50, 0.50, 0.50, 0.50,-0.20,-0.20,-0.20,-0.20,-0.20,-0.20,-0.20, 0.50, 0.50, 0.50,0.00};

    int n = (int) goal->locations.poses.size();

    ROS_INFO("received %d locations to visit in %f minutes", n, 0.0);

    ROS_INFO("undocking...");

    //Undocking
    current_goal.action_server = "undocking";
    current_goal.target_pose.header.frame_id = "map";
    ac_nav_ptr->sendGoal(current_goal);
    ac_nav_ptr->waitForResult(ros::Duration(0.0));

    //    actionlib::SimpleClientGoalState state;
    unsigned int retries = 0;

    if (ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)//undocking was sucessful
    {
        current_goal.action_server = "move_base";
        current_goal.target_pose.header.frame_id = "map";

        ROS_INFO("Undocking sucessful! Starting exploration run.");
        for(int i = 0; i < n; i++)
        {
            char cr_goal[10];
            sprintf(cr_goal, "%d/%d", i, n);
            feedback.current_goal = cr_goal;
            feedback.time_remaining = 0;//TODO
            as->publishFeedback(feedback);

            current_goal.target_pose.pose = goal->locations.poses[i];
            ROS_INFO("moving to location %d/%d -> (%f,%f)",  i, n, goal->locations.poses[i].position.x, goal->locations.poses[i].position.y);
            ac_nav_ptr->sendGoal(current_goal);

            while(ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::ACTIVE || ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::PENDING);

            if(ac_nav_ptr->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)//if it fails tries more 3 times (recovery behaviours)
            {
                while(retries < 3 && ac_nav_ptr->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
                {
                    ac_nav_ptr->sendGoal(current_goal);
                    while(ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::ACTIVE || ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::PENDING);

                }
                retries++;
            }

            if (i>0 && i < n-1){
                if (ac_nav_ptr->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                    ROS_INFO("Monitored navigation: SUCCEEDED! taking measurements!");
                else
                    ROS_WARN("Point no reacheable! Taking measurements and moving to the next point...");

                point = 0;
                movePtu(pan[point],tilt[point]);

                ros::spinOnce();
                while (ros::ok() && point < numPoints)
                {
                    measure_srv.request.stamp = 0.0;
                    if (ptuMovementFinished > 10)
                    {
                        if(measure_client_ptr->call(measure_srv))
                        {
                            ROS_INFO("Measure added to grid!");
                        }
                        else
                        {
                            ROS_ERROR("Failed to call measure service");
                            exit(1);
                        }

                        point++;
                        movePtu(pan[point],tilt[point]);
                        ros::spinOnce();
                        usleep(500000);
                        if(drawCells){
                            visualize_srv.request.red = visualize_srv.request.blue = 0.0;
                            visualize_srv.request.green = visualize_srv.request.alpha = 1.0;
                            visualize_srv.request.minProbability = 0.9;
                            visualize_srv.request.maxProbability = 1.0;
                            visualize_srv.request.name = "occupied";
                            visualize_srv.request.type = 0;
                            visualize_client_ptr->call(visualize_srv);
                            ros::spinOnce();
                            usleep(100000);
                            if (drawEmptyCells){
                                visualize_srv.request.green = 0.0;
                                visualize_srv.request.red = 1.0;
                                visualize_srv.request.minProbability = 0.0;
                                visualize_srv.request.maxProbability = 0.1;
                                visualize_srv.request.alpha = 0.005;
                                visualize_srv.request.name = "free";
                                visualize_srv.request.type = 0;
                                visualize_client_ptr->call(visualize_srv);
                                ros::spinOnce();
                                usleep(100000);
                            }
                        }
                    }
                    ros::spinOnce();
                }

            }
            movePtu(0.0,0.0);
        }

        ROS_INFO("my work is done! going to charging station...");

        //Docking
        current_goal.action_server = "docking";
        current_goal.target_pose.header.frame_id = "map";
        ac_nav_ptr->sendGoal(current_goal);
        ac_nav_ptr->waitForResult(ros::Duration(0.0));

        if (ac_nav_ptr->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)//docking was sucessful
            ROS_ERROR("docking failed!");
    }
    else{
        ROS_ERROR("undocking failed!");
    }

    as->setSucceeded();

}

int main(int argc,char *argv[])
{
    ros::init(argc, argv, "exploration_executioner");
    ros::NodeHandle n;

    ros::NodeHandle nh("~");

    Server server(n, "executioner", boost::bind(&execute, _1, &server), false);
    ROS_INFO("starting server...");
    server.start();

    actionlib::SimpleActionClient<strands_navigation_msgs::MonitoredNavigationAction> ac_nav("monitored_navigation",true);
    ac_nav_ptr = &ac_nav;
    ac_nav.waitForServer();

    //Subscribers
    ros::Subscriber ptu_sub = n.subscribe("/ptu/state", 10, ptuCallback);
    ptu.name.resize(3);
    ptu.position.resize(3);
    ptu.velocity.resize(3);
    ptu_pub = n.advertise<sensor_msgs::JointState>("/ptu/cmd", 10);

    //measure service client
    ros::ServiceClient measure_client = n.serviceClient<spatiotemporalexploration::AddView>("/fremenGrid/depth");
    measure_client_ptr = &measure_client;

    //vizualize client
    ros::ServiceClient visualize_client = n.serviceClient<spatiotemporalexploration::Visualize>("/fremenGrid/visualize");
    visualize_client_ptr = &visualize_client;

    //Dynamic Reconfigure
    dynamic_reconfigure::ReconfigureRequest srv_req;
    dynamic_reconfigure::ReconfigureResponse srv_resp;
    dynamic_reconfigure::DoubleParameter double_param;
    dynamic_reconfigure::Config conf;

    double_param.name = "yaw_goal_tolerance";
    double_param.value = 3.0;
    conf.doubles.push_back(double_param);

    srv_req.config = conf;

    ros::service::call("/move_base/DWAPlannerROS/set_parameters", srv_req, srv_resp);

    ROS_INFO("server started");

    ros::spin();

    return 0;
}
<|endoftext|>


<commit_before>#include "exportcorrelationmatrix.h"
#include "datafactory.h"

EAbstractAnalytic::ArgumentType ExportCorrelationMatrix::getArgumentData(int argument)
{
   // use type declaration
   using Type = EAbstractAnalytic::ArgumentType;

   // figure out which argument is being queried and return its type
   switch (argument)
   {
   case ClusterData: return Type::DataIn;
   case CorrelationData: return Type::DataIn;
   case OutputFile: return Type::FileOut;
   default: return Type::Bool;
   }
}

QVariant ExportCorrelationMatrix::getArgumentData(int argument, Role role)
{
   // use role declaration
   using Role = EAbstractAnalytic::Role;

   // figure out which role is being queried
   switch (role)
   {
   case Role::CommandLineName:
      // figure out which argument is being queried and return command line name
      switch (argument)
      {
      case ClusterData: return QString("clus");
      case CorrelationData: return QString("corr");
      case OutputFile: return QString("output");
      default: return QString();
      }
   case Role::Title:
      // figure out which argument is being queried and return title
      switch (argument)
      {
      case ClusterData: return tr("Cluster Matrix:");
      case CorrelationData: return tr("Correlation Matrix:");
      case OutputFile: return tr("Output File:");
      default: return QString();
      }
   case Role::WhatsThis:
      // figure out which argument is being queried and return "What's This?" text
      switch (argument)
      {
      case ClusterData: return tr("Input cluster matrix containing cluster composition data.");
      case CorrelationData: return tr("Input correlation matrix containing correlation data.");
      case OutputFile: return tr("Raw output text file that will contain gene pair correlation data.");
      default: return QString();
      }
   case Role::DataType:
      // if this is input data argument return data type else return nothing
      switch (argument)
      {
      case ClusterData: return DataFactory::CCMatrixType;
      case CorrelationData: return DataFactory::CorrelationMatrixType;
      default: return QString();
      }
   case Role::FileFilters:
      // if this is output file argument return file filter else return nothing
      switch (argument)
      {
      case OutputFile: return tr("Raw text file %1").arg("(*.txt)");
      default: return QString();
      }
   default:
      return QVariant();
   }
}

void ExportCorrelationMatrix::setArgument(int argument, EAbstractData* data)
{
   // if argument is input data set it
   if ( argument == ClusterData )
   {
      _ccMatrix = dynamic_cast<CCMatrix*>(data);
   }
   else if ( argument == CorrelationData )
   {
      _cMatrix = dynamic_cast<CorrelationMatrix*>(data);
   }
}

void ExportCorrelationMatrix::setArgument(int argument, QFile* file)
{
   // if argument is output file set it
   if ( argument == OutputFile )
   {
      _output = file;
   }
}

bool ExportCorrelationMatrix::initialize()
{
   // make sure input and output arguments were set
   if ( !_ccMatrix || !_cMatrix || !_output )
   {
      E_MAKE_EXCEPTION(e);
      e.setTitle(QObject::tr("Argument Error"));
      e.setDetails(QObject::tr("Did not get valid input and/or output arguments."));
      throw e;
   }

   // do not have data pre-allocate
   return false;
}

void ExportCorrelationMatrix::runSerial()
{
   // initialize percent complete and steps
   int lastPercent {0};
   qint64 steps {0};
   qint64 totalSteps {_ccMatrix->geneSize()*(_ccMatrix->geneSize() - 1)/2};

   // initialize xy gene indexes
   GenePair::Vector vector;
   CCMatrix::Pair ccPair(_ccMatrix);
   CorrelationMatrix::Pair cPair(_cMatrix);

   // initialize workspace
   QString sampleMask(_ccMatrix->sampleSize());

   // create text stream to output file and write until end reached
   QTextStream stream(_output);
   stream.setRealNumberPrecision(6);

   // increment through all gene pairs
   while ( vector.geneX() < _ccMatrix->geneSize() )
   {
      // make sure interruption is not requested
      if ( isInterruptionRequested() )
      {
         return;
      }

      // read cluster and correlation data for gene pair
      ccPair.read(vector);
      cPair.read(vector);

      // write gene pair data to output file
      for ( int cluster = 0; cluster < cPair.clusterSize(); cluster++ )
      {
         int numSamples = 0;
         int numMissing = 0;
         int numPostOutliers = 0;
         int numPreOutliers = 0;
         int numThreshold = 0;

         if ( ccPair.clusterSize() > 1 )
         {
            // compute summary statistics
            for ( int i = 0; i < _ccMatrix->sampleSize(); i++ )
            {
               switch ( ccPair.at(cluster, i) )
               {
               case 1:
                  numSamples++;
                  break;
               case 6:
                  numThreshold++;
                  break;
               case 7:
                  numPreOutliers++;
                  break;
               case 8:
                  numPostOutliers++;
                  break;
               case 9:
                  numMissing++;
                  break;
               }
            }

            // write sample mask to string
            for ( int i = 0; i < _ccMatrix->sampleSize(); i++ )
            {
               sampleMask[i] = '0' + ccPair.at(cluster, i);
            }
         }
         else
         {
            // initialize sample mask to all 0's
            sampleMask.fill('0');
         }

         // write cluster to output file
         stream
            << vector.geneX()
            << "\t" << vector.geneY()
            << "\t" << cluster
            << "\t" << cPair.clusterSize()
            << "\t" << numSamples
            << "\t" << numMissing
            << "\t" << numPostOutliers
            << "\t" << numPreOutliers
            << "\t" << numThreshold
            << "\t" << cPair.at(cluster, 0)
            << "\t" << sampleMask
            << "\n";
      }

      // increment to next pair
      ++vector;

      // increment steps and calculate percent complete
      ++steps;
      qint64 newPercent {100*steps/totalSteps};

      // check to see if percent has changed
      if ( newPercent != lastPercent )
      {
         // update percent complete and emit progressed signal
         lastPercent = newPercent;
         emit progressed(lastPercent);
      }
   }

   // make sure writing output file worked
   if ( stream.status() != QTextStream::Ok )
   {
      E_MAKE_EXCEPTION(e);
      e.setTitle(tr("File IO Error"));
      e.setDetails(tr("Qt Text Stream encountered an unknown error."));
      throw e;
   }
}
<commit_msg>Changed ExportCorrelationMatrix analytic to be faster<commit_after>#include "exportcorrelationmatrix.h"
#include "datafactory.h"

EAbstractAnalytic::ArgumentType ExportCorrelationMatrix::getArgumentData(int argument)
{
   // use type declaration
   using Type = EAbstractAnalytic::ArgumentType;

   // figure out which argument is being queried and return its type
   switch (argument)
   {
   case ClusterData: return Type::DataIn;
   case CorrelationData: return Type::DataIn;
   case OutputFile: return Type::FileOut;
   default: return Type::Bool;
   }
}

QVariant ExportCorrelationMatrix::getArgumentData(int argument, Role role)
{
   // use role declaration
   using Role = EAbstractAnalytic::Role;

   // figure out which role is being queried
   switch (role)
   {
   case Role::CommandLineName:
      // figure out which argument is being queried and return command line name
      switch (argument)
      {
      case ClusterData: return QString("clus");
      case CorrelationData: return QString("corr");
      case OutputFile: return QString("output");
      default: return QString();
      }
   case Role::Title:
      // figure out which argument is being queried and return title
      switch (argument)
      {
      case ClusterData: return tr("Cluster Matrix:");
      case CorrelationData: return tr("Correlation Matrix:");
      case OutputFile: return tr("Output File:");
      default: return QString();
      }
   case Role::WhatsThis:
      // figure out which argument is being queried and return "What's This?" text
      switch (argument)
      {
      case ClusterData: return tr("Input cluster matrix containing cluster composition data.");
      case CorrelationData: return tr("Input correlation matrix containing correlation data.");
      case OutputFile: return tr("Raw output text file that will contain gene pair correlation data.");
      default: return QString();
      }
   case Role::DataType:
      // if this is input data argument return data type else return nothing
      switch (argument)
      {
      case ClusterData: return DataFactory::CCMatrixType;
      case CorrelationData: return DataFactory::CorrelationMatrixType;
      default: return QString();
      }
   case Role::FileFilters:
      // if this is output file argument return file filter else return nothing
      switch (argument)
      {
      case OutputFile: return tr("Raw text file %1").arg("(*.txt)");
      default: return QString();
      }
   default:
      return QVariant();
   }
}

void ExportCorrelationMatrix::setArgument(int argument, EAbstractData* data)
{
   // if argument is input data set it
   if ( argument == ClusterData )
   {
      _ccMatrix = dynamic_cast<CCMatrix*>(data);
   }
   else if ( argument == CorrelationData )
   {
      _cMatrix = dynamic_cast<CorrelationMatrix*>(data);
   }
}

void ExportCorrelationMatrix::setArgument(int argument, QFile* file)
{
   // if argument is output file set it
   if ( argument == OutputFile )
   {
      _output = file;
   }
}

bool ExportCorrelationMatrix::initialize()
{
   // make sure input and output arguments were set
   if ( !_ccMatrix || !_cMatrix || !_output )
   {
      E_MAKE_EXCEPTION(e);
      e.setTitle(QObject::tr("Argument Error"));
      e.setDetails(QObject::tr("Did not get valid input and/or output arguments."));
      throw e;
   }

   // do not have data pre-allocate
   return false;
}

void ExportCorrelationMatrix::runSerial()
{
   // initialize percent complete and steps
   int lastPercent {0};
   qint64 steps {0};
   qint64 totalSteps {_cMatrix->size()};

   // initialize pair iterators
   CorrelationMatrix::Pair cPair(_cMatrix);
   CCMatrix::Pair ccPair(_ccMatrix);

   // initialize workspace
   QString sampleMask(_ccMatrix->sampleSize(), '0');

   // create text stream to output file and write until end reached
   QTextStream stream(_output);
   stream.setRealNumberPrecision(6);

   // increment through all gene pairs
   while ( cPair.hasNext() )
   {
      // make sure interruption is not requested
      if ( isInterruptionRequested() )
      {
         return;
      }

      // read next gene pair
      cPair.readNext();

      if ( cPair.clusterSize() > 1 )
      {
         ccPair.read(cPair.vector());
      }

      // write gene pair data to output file
      for ( int cluster = 0; cluster < cPair.clusterSize(); cluster++ )
      {
         float correlation = cPair.at(cluster, 0);
         int numSamples = 0;
         int numMissing = 0;
         int numPostOutliers = 0;
         int numPreOutliers = 0;
         int numThreshold = 0;

         if ( ccPair.clusterSize() > 1 )
         {
            // compute summary statistics
            for ( int i = 0; i < _ccMatrix->sampleSize(); i++ )
            {
               switch ( ccPair.at(cluster, i) )
               {
               case 1:
                  numSamples++;
                  break;
               case 6:
                  numThreshold++;
                  break;
               case 7:
                  numPreOutliers++;
                  break;
               case 8:
                  numPostOutliers++;
                  break;
               case 9:
                  numMissing++;
                  break;
               }
            }

            // write sample mask to string
            for ( int i = 0; i < _ccMatrix->sampleSize(); i++ )
            {
               sampleMask[i] = '0' + ccPair.at(cluster, i);
            }
         }
         else
         {
            // initialize sample mask to all 0's
            sampleMask.fill('0');
         }

         // write cluster to output file
         stream
            << cPair.vector().geneX()
            << "\t" << cPair.vector().geneY()
            << "\t" << cluster
            << "\t" << cPair.clusterSize()
            << "\t" << numSamples
            << "\t" << numMissing
            << "\t" << numPostOutliers
            << "\t" << numPreOutliers
            << "\t" << numThreshold
            << "\t" << correlation
            << "\t" << sampleMask
            << "\n";
      }

      // increment steps and calculate percent complete
      ++steps;
      qint64 newPercent {100*steps/totalSteps};

      // check to see if percent has changed
      if ( newPercent != lastPercent )
      {
         // update percent complete and emit progressed signal
         lastPercent = newPercent;
         emit progressed(lastPercent);
      }
   }

   // make sure writing output file worked
   if ( stream.status() != QTextStream::Ok )
   {
      E_MAKE_EXCEPTION(e);
      e.setTitle(tr("File IO Error"));
      e.setDetails(tr("Qt Text Stream encountered an unknown error."));
      throw e;
   }
}
<|endoftext|>