<commit_before>/*
 * Universe
 *
 * Copyright 2015 Lubosz Sarnecki <lubosz@gmail.com>
 *
 */

#include "Renderer.h"

#include "util.h"
#include <gli/gli.hpp>

Renderer::Renderer(int width, int height) {
    translate_z = -0.1f;
    rotate_x = 0.0;
    rotate_y = 0.0;

    if (gl3wInit()) {
        fprintf(stderr, "failed to initialize gl3w\n");
        return;
    }
    if (!gl3wIsSupported(4, 5)) {
        fprintf(stderr, "OpenGL 4.5 not supported\n");
        return;
    }

    printContextInfo();
    initShaders();
    glUseProgram(shader_programm);

    glEnable(GL_VERTEX_PROGRAM_POINT_SIZE);

    GLuint tex = initTexture("media/cloud.dds");
    glBindTexture(GL_TEXTURE_2D, tex);

    GLint texLoc = glGetUniformLocation(shader_programm, "cloud");
    glUniform1i(texLoc, tex-1);

    updateModel();
    updateProjection(width, height);

    glClearColor(0.0, 0.0, 0.0, 1.0);
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    glBindVertexArray(vao);

    glError;
}

Renderer::~Renderer() {}

void Renderer::draw(GLuint positionVBO, GLuint colorVBO,
                    GLuint massVBO, int particleCount) {
  // render the particles from VBOs
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glGenVertexArrays(1, &vao);
  glBindVertexArray(vao);
  glEnableVertexAttribArray(0);
  glBindBuffer(GL_ARRAY_BUFFER, positionVBO);
  glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 0, NULL);

  glEnableVertexAttribArray(1);
  glBindBuffer(GL_ARRAY_BUFFER, colorVBO);
  glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, NULL);

  glEnableVertexAttribArray(2);
  glBindBuffer(GL_ARRAY_BUFFER, massVBO);
  glVertexAttribPointer(2, 1, GL_FLOAT, GL_FALSE, 0, NULL);

  glDrawArrays(GL_POINTS, 0, particleCount);
}

void Renderer::printContextInfo() {
    printf("GL_RENDERER: %s\n", glGetString(GL_RENDERER));
    printf("GL_VERSION: %s\n", glGetString(GL_VERSION));
    printf("GL_SHADING_LANGUAGE_VERSION: %s\n",
           glGetString(GL_SHADING_LANGUAGE_VERSION));
}

GLuint Renderer::initTexture(char const* Filename) {
    gli::texture Texture = gli::load(Filename);
    if (Texture.empty())
        return 0;

    gli::gl GL;
    gli::gl::format const Format = GL.translate(Texture.format());
    GLenum Target = GL.translate(Texture.target());

    GLuint TextureName = 0;
    glGenTextures(1, &TextureName);
    glBindTexture(Target, TextureName);
    glTexParameteri(Target, GL_TEXTURE_BASE_LEVEL, 0);
    glTexParameteri(Target,
        GL_TEXTURE_MAX_LEVEL, static_cast<GLint>(Texture.levels() - 1));

    glm::tvec3<GLsizei> const Dimensions(Texture.dimensions());
    GLsizei const FaceTotal =
            static_cast<GLsizei>(Texture.layers() * Texture.faces());

    glTexStorage2D(
                Target, static_cast<GLint>(Texture.levels()), Format.Internal,
                Dimensions.x,
                Texture.target() == gli::TARGET_2D ? Dimensions.y : FaceTotal);

    for (std::size_t Layer = 0;
         Layer < Texture.layers(); ++Layer)
        for (std::size_t Face = 0;
             Face < Texture.faces(); ++Face)
            for (std::size_t Level = 0;
                 Level < Texture.levels(); ++Level) {
                glm::tvec3<GLsizei> Dimensions(Texture.dimensions(Level));

                /*
                printf("layer %ld, face %ld, level %ld, iscompressed %d\n",
                       Layer, Face, Level,
                       gli::is_compressed(Texture.format()));
                */
                if (gli::is_compressed(Texture.format()))
                    glCompressedTexSubImage2D(
                                Target, static_cast<GLint>(Level),
                                0, 0,
                                Dimensions.x,
                                Dimensions.y,
                                Format.Internal,
                                static_cast<GLsizei>(Texture.size(Level)),
                                Texture.data(Layer, Face, Level));
                else
                    glTexSubImage2D(
                                Target, static_cast<GLint>(Level),
                                0, 0,
                                Dimensions.x,
                                Dimensions.y,
                                Format.External, Format.Type,
                                Texture.data(Layer, Face, Level));
            }
    return TextureName;
}

void Renderer::initShaders() {
    shaders.push_back(readFile("gpu/MVP.vert"));
    shaders.push_back(readFile("gpu/color.frag"));

    const char* vertex[] = {shaders[0].c_str()};
    const char* fragment[] = {shaders[1].c_str()};

    GLuint vs = glCreateShader(GL_VERTEX_SHADER);
    glShaderSource(vs, 1, vertex, NULL);
    glCompileShader(vs);
    GLuint fs = glCreateShader(GL_FRAGMENT_SHADER);
    glShaderSource(fs, 1, fragment, NULL);
    glCompileShader(fs);

    shader_programm = glCreateProgram();
    glAttachShader(shader_programm, fs);
    glAttachShader(shader_programm, vs);
    glLinkProgram(shader_programm);
}

void Renderer::rotate(float x, float y) {
    rotate_x += y * 0.2;
    rotate_y += x * 0.2;
    updateModel();
}

void Renderer::translate(float z) {
    translate_z += z * 0.1;
    updateModel();
}

void Renderer::updateModel() {
    model = glm::mat4(1.0f);
    model = glm::translate(glm::mat4(1.0f), glm::vec3(0.0f, 0.0f, translate_z));
    model = glm::rotate(model, rotate_x, glm::vec3(1.0f, 0.0f, 0.0f));
    model = glm::rotate(model, rotate_y, glm::vec3(0.0f, 1.0f, 0.0f));
    updateMVP();
}

void Renderer::updateProjection(int width, int height) {
    glViewport(0, 0, width, height);
    float aspect = static_cast<GLfloat>(width)
            / static_cast<GLfloat>(height);
    projection = glm::perspective(45.0f, aspect, 0.01f, 10000.f);
    updateMVP();
}

void Renderer::updateMVP() {
    glUniformMatrix4fv(
                glGetUniformLocation(shader_programm, "modelMatrix"),
                1, GL_FALSE, &model[0][0]);
    glUniformMatrix4fv(
                glGetUniformLocation(shader_programm, "projectionMatrix"),
                1, GL_FALSE, &projection[0][0]);
}

void Renderer::checkGlError(const char* file, int line) {
    GLenum err(glGetError());

    while (err != GL_NO_ERROR) {
        std::string error;
        switch (err) {
            case GL_INVALID_OPERATION:
                error = "INVALID_OPERATION";
                break;
            case GL_INVALID_ENUM:
                error = "INVALID_ENUM";
                break;
            case GL_INVALID_VALUE:
                error = "INVALID_VALUE";
                break;
            case GL_OUT_OF_MEMORY:
                error = "OUT_OF_MEMORY";
                break;
            case GL_INVALID_FRAMEBUFFER_OPERATION:
                error = "INVALID_FRAMEBUFFER_OPERATION";
                break;
            default:
                error = "Unknown error";
                break;
        }
        std::cout
                << "GL ERROR: GL_"
                << error << " "
                << file << " "
                << line << "\n";
        err = glGetError();
    }
}

GLuint Renderer::createVBO(
        const void* data,
        int dataSize,
        GLenum target,
        GLenum usage) {
    GLuint id;
    glGenBuffers(1, &id);
    glBindBuffer(target, id);
    glBufferData(target, dataSize, data, usage);

    // check data size in VBO is same as input array
    // if not return 0 and delete VBO
    int bufferSize = 0;
    glGetBufferParameteriv(target, GL_BUFFER_SIZE, &bufferSize);
    if (dataSize != bufferSize) {
        glDeleteBuffers(1, &id);
        id = 0;
        std::cout << "[createVBO()] Data size is mismatch with input array\n";
    }
    glBindBuffer(target, 0);
    return id;
}
<commit_msg>Renderer: change initial camera position and scroll strep<commit_after>/*
 * Universe
 *
 * Copyright 2015 Lubosz Sarnecki <lubosz@gmail.com>
 *
 */

#include "Renderer.h"

#include "util.h"
#include <gli/gli.hpp>

Renderer::Renderer(int width, int height) {
    translate_z = -0.5f;
    rotate_x = 0.0;
    rotate_y = 0.0;

    if (gl3wInit()) {
        fprintf(stderr, "failed to initialize gl3w\n");
        return;
    }
    if (!gl3wIsSupported(4, 5)) {
        fprintf(stderr, "OpenGL 4.5 not supported\n");
        return;
    }

    printContextInfo();
    initShaders();
    glUseProgram(shader_programm);

    glEnable(GL_VERTEX_PROGRAM_POINT_SIZE);

    GLuint tex = initTexture("media/cloud.dds");
    glBindTexture(GL_TEXTURE_2D, tex);

    GLint texLoc = glGetUniformLocation(shader_programm, "cloud");
    glUniform1i(texLoc, tex-1);

    updateModel();
    updateProjection(width, height);

    glClearColor(0.0, 0.0, 0.0, 1.0);
    glDisable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    glBindVertexArray(vao);

    glError;
}

Renderer::~Renderer() {}

void Renderer::draw(GLuint positionVBO, GLuint colorVBO,
                    GLuint massVBO, int particleCount) {
  // render the particles from VBOs
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glGenVertexArrays(1, &vao);
  glBindVertexArray(vao);
  glEnableVertexAttribArray(0);
  glBindBuffer(GL_ARRAY_BUFFER, positionVBO);
  glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 0, NULL);

  glEnableVertexAttribArray(1);
  glBindBuffer(GL_ARRAY_BUFFER, colorVBO);
  glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, NULL);

  glEnableVertexAttribArray(2);
  glBindBuffer(GL_ARRAY_BUFFER, massVBO);
  glVertexAttribPointer(2, 1, GL_FLOAT, GL_FALSE, 0, NULL);

  glDrawArrays(GL_POINTS, 0, particleCount);
}

void Renderer::printContextInfo() {
    printf("GL_RENDERER: %s\n", glGetString(GL_RENDERER));
    printf("GL_VERSION: %s\n", glGetString(GL_VERSION));
    printf("GL_SHADING_LANGUAGE_VERSION: %s\n",
           glGetString(GL_SHADING_LANGUAGE_VERSION));
}

GLuint Renderer::initTexture(char const* Filename) {
    gli::texture Texture = gli::load(Filename);
    if (Texture.empty())
        return 0;

    gli::gl GL;
    gli::gl::format const Format = GL.translate(Texture.format());
    GLenum Target = GL.translate(Texture.target());

    GLuint TextureName = 0;
    glGenTextures(1, &TextureName);
    glBindTexture(Target, TextureName);
    glTexParameteri(Target, GL_TEXTURE_BASE_LEVEL, 0);
    glTexParameteri(Target,
        GL_TEXTURE_MAX_LEVEL, static_cast<GLint>(Texture.levels() - 1));

    glm::tvec3<GLsizei> const Dimensions(Texture.dimensions());
    GLsizei const FaceTotal =
            static_cast<GLsizei>(Texture.layers() * Texture.faces());

    glTexStorage2D(
                Target, static_cast<GLint>(Texture.levels()), Format.Internal,
                Dimensions.x,
                Texture.target() == gli::TARGET_2D ? Dimensions.y : FaceTotal);

    for (std::size_t Layer = 0;
         Layer < Texture.layers(); ++Layer)
        for (std::size_t Face = 0;
             Face < Texture.faces(); ++Face)
            for (std::size_t Level = 0;
                 Level < Texture.levels(); ++Level) {
                glm::tvec3<GLsizei> Dimensions(Texture.dimensions(Level));

                /*
                printf("layer %ld, face %ld, level %ld, iscompressed %d\n",
                       Layer, Face, Level,
                       gli::is_compressed(Texture.format()));
                */
                if (gli::is_compressed(Texture.format()))
                    glCompressedTexSubImage2D(
                                Target, static_cast<GLint>(Level),
                                0, 0,
                                Dimensions.x,
                                Dimensions.y,
                                Format.Internal,
                                static_cast<GLsizei>(Texture.size(Level)),
                                Texture.data(Layer, Face, Level));
                else
                    glTexSubImage2D(
                                Target, static_cast<GLint>(Level),
                                0, 0,
                                Dimensions.x,
                                Dimensions.y,
                                Format.External, Format.Type,
                                Texture.data(Layer, Face, Level));
            }
    return TextureName;
}

void Renderer::initShaders() {
    shaders.push_back(readFile("gpu/MVP.vert"));
    shaders.push_back(readFile("gpu/color.frag"));

    const char* vertex[] = {shaders[0].c_str()};
    const char* fragment[] = {shaders[1].c_str()};

    GLuint vs = glCreateShader(GL_VERTEX_SHADER);
    glShaderSource(vs, 1, vertex, NULL);
    glCompileShader(vs);
    GLuint fs = glCreateShader(GL_FRAGMENT_SHADER);
    glShaderSource(fs, 1, fragment, NULL);
    glCompileShader(fs);

    shader_programm = glCreateProgram();
    glAttachShader(shader_programm, fs);
    glAttachShader(shader_programm, vs);
    glLinkProgram(shader_programm);
}

void Renderer::rotate(float x, float y) {
    rotate_x += y * 0.2;
    rotate_y += x * 0.2;
    updateModel();
}

void Renderer::translate(float z) {
    translate_z += z * 0.01;
    updateModel();
}

void Renderer::updateModel() {
    model = glm::mat4(1.0f);
    model = glm::translate(glm::mat4(1.0f), glm::vec3(0.0f, 0.0f, translate_z));
    model = glm::rotate(model, rotate_x, glm::vec3(1.0f, 0.0f, 0.0f));
    model = glm::rotate(model, rotate_y, glm::vec3(0.0f, 1.0f, 0.0f));
    updateMVP();
}

void Renderer::updateProjection(int width, int height) {
    glViewport(0, 0, width, height);
    float aspect = static_cast<GLfloat>(width)
            / static_cast<GLfloat>(height);
    projection = glm::perspective(45.0f, aspect, 0.01f, 10000.f);
    updateMVP();
}

void Renderer::updateMVP() {
    glUniformMatrix4fv(
                glGetUniformLocation(shader_programm, "modelMatrix"),
                1, GL_FALSE, &model[0][0]);
    glUniformMatrix4fv(
                glGetUniformLocation(shader_programm, "projectionMatrix"),
                1, GL_FALSE, &projection[0][0]);
}

void Renderer::checkGlError(const char* file, int line) {
    GLenum err(glGetError());

    while (err != GL_NO_ERROR) {
        std::string error;
        switch (err) {
            case GL_INVALID_OPERATION:
                error = "INVALID_OPERATION";
                break;
            case GL_INVALID_ENUM:
                error = "INVALID_ENUM";
                break;
            case GL_INVALID_VALUE:
                error = "INVALID_VALUE";
                break;
            case GL_OUT_OF_MEMORY:
                error = "OUT_OF_MEMORY";
                break;
            case GL_INVALID_FRAMEBUFFER_OPERATION:
                error = "INVALID_FRAMEBUFFER_OPERATION";
                break;
            default:
                error = "Unknown error";
                break;
        }
        std::cout
                << "GL ERROR: GL_"
                << error << " "
                << file << " "
                << line << "\n";
        err = glGetError();
    }
}

GLuint Renderer::createVBO(
        const void* data,
        int dataSize,
        GLenum target,
        GLenum usage) {
    GLuint id;
    glGenBuffers(1, &id);
    glBindBuffer(target, id);
    glBufferData(target, dataSize, data, usage);

    // check data size in VBO is same as input array
    // if not return 0 and delete VBO
    int bufferSize = 0;
    glGetBufferParameteriv(target, GL_BUFFER_SIZE, &bufferSize);
    if (dataSize != bufferSize) {
        glDeleteBuffers(1, &id);
        id = 0;
        std::cout << "[createVBO()] Data size is mismatch with input array\n";
    }
    glBindBuffer(target, 0);
    return id;
}
<|endoftext|>


<commit_before>#include "Renderer.hpp"

static GLfloat vertices[] = {
    1, 1,0,  1,-1,0, -1,1,0,
    1,-1,0, -1,-1,0, -1,1,0
};
static GLfloat uvs[] = {
    1,0, 1,1, 0,0,
    1,1, 0,1, 0,0
};

const char * Renderer::defaultVertexShader =
        "#version 330 core\n"
        "\n"
        "in vec3 position;\n"
        "in vec2 texCoord;\n"
        "\n"
        "out vec2  uv;\n"
        "\n"
        "void main(){\n"
        "   gl_Position = vec4(position, 1);\n"
        "   uv = texCoord;\n"
        "}";

const char * Renderer::defaultFragmentShader =
        "#version 330 core\n"
        "\n"
        "out vec4 color;\n"
        "\n"
        "in vec2 uv;\n"
        "uniform float time;\n"
        "uniform vec2 mouse;\n"
        "uniform float ration;\n"
        "\n"
        "void main(){\n"
        "     color = vec4(cos(uv.x * 5 - time / 1000) / 2 + .5, 0, sin(uv.x * 5 - time / 1000) / 2 + .5, 1);\n"
        "}";

/**
 * @brief Renderer::Renderer
 * @param parent Parent object of the render window
 *
 * Create a new Renderer with default shader
 */
Renderer::Renderer(QWindow *parent) : Renderer::Renderer("new", defaultFragmentShader, parent){ }

/**
 * @brief Renderer::Renderer
 * @param filename Name that should shown in the title
 * @param instructions Shader code for execution
 * @param parent Parent object of the render window
 *
 * Create a new Renderer with given code and set filename as title
 */
Renderer::Renderer(const QString &filename, const QString &instructions, QWindow *parent) :
    QWindow(parent),
    clearColor(Qt::black),
    context(0), device(0),
    time(0),
    pendingUpdate(false),
    vao(0), uvBuffer(0), audioLeftTexture(0), audioRightTexture(0),
    vertexAttr(0), uvAttr(0), timeUniform(0),
    shaderProgram(0),
    fragmentSource(instructions)
{
    setTitle(filename);

    m_logger = new QOpenGLDebugLogger( this );

    connect(m_logger, SIGNAL(messageLogged(QOpenGLDebugMessage)),
             this, SLOT(onMessageLogged(QOpenGLDebugMessage)),
             Qt::DirectConnection );

    setSurfaceType(QWindow::OpenGLSurface);

    time = new QTime();
    time->start();

    audio = new AudioInputProcessor(this);
    connect(audio, SIGNAL(processData(QByteArray)), this, SLOT(updateAudioData(QByteArray)));
    audio->start();

    QSurfaceFormat format;
    format.setMajorVersion(3);
    format.setMinorVersion(3);
    format.setSamples(4);
    format.setProfile(QSurfaceFormat::CoreProfile);
    setFormat(format);
}

/**
 * @brief Renderer::~Renderer
 *
 * Free resources
 */
Renderer::~Renderer(){
    glDeleteBuffers(1, &uvBuffer);
    glDeleteTextures(1, &audioLeftTexture);
    glDeleteTextures(1, &audioRightTexture);
    delete time;
    delete vao;
    delete device;
    delete m_logger;
}

/**
 * @brief Renderer::init
 * @return True on success, otherwise false
 *
 * Allocates grafic memory and initialize the shader program
 */
bool Renderer::init(){
    delete vao;
    vao = new QOpenGLVertexArrayObject(this);
    vao->create();
    vao->bind();

    glDeleteBuffers(1, &vertexBuffer);
    glGenBuffers(1, &vertexBuffer);
    glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    glDeleteBuffers(1, &uvBuffer);
    glGenBuffers(1, &uvBuffer);
    glBindBuffer(GL_ARRAY_BUFFER, uvBuffer);
    glBufferData(GL_ARRAY_BUFFER, sizeof(uvs), uvs, GL_STATIC_DRAW);

    glEnable(GL_TEXTURE_1D);

    glDeleteTextures(1, &audioLeftTexture);
    glGenTextures(1, &audioLeftTexture);
    glDeleteTextures(1, &audioRightTexture);
    glGenTextures(1, &audioRightTexture);

    glClearColor(0,0,.3,1);
    bool result = initShaders(fragmentSource);

    vao->release();

    return result;
}

/**
 * @brief Renderer::initShaders
 * @param fragmentShader Code to compile as shader
 * @return True on success, otherwise false
 *
 * Initialze and compile the shader program
 */
bool Renderer::initShaders(const QString &fragmentShader){
    QOpenGLShaderProgram *newShaderProgram = new QOpenGLShaderProgram(this);
    bool hasError = false;
    QString error = "";

    if(!hasError && !newShaderProgram->addShaderFromSourceCode(QOpenGLShader::Vertex, defaultVertexShader)){
        hasError = true;
        error = newShaderProgram->log();

        qWarning() << tr("Failed to compile default shader.");
    }
    if(!hasError && !newShaderProgram->addShaderFromSourceCode(QOpenGLShader::Fragment,fragmentShader)){
        hasError = true;
        error = newShaderProgram->log();

        if(fragmentShader == defaultFragmentShader)
            qWarning() << tr("Failed to compile default shader.");
        else if(shaderProgram == 0)
            initShaders(defaultFragmentShader);
    }
    if(!hasError && !newShaderProgram->link()){
        hasError = true;
        error = newShaderProgram->log();

        if(fragmentShader == defaultFragmentShader)
            qWarning() << tr("Failed to compile default shader.");
        else if(shaderProgram == 0)
            initShaders(defaultFragmentShader);
    }
    if(hasError){
        delete newShaderProgram;

        QRegExp errorline(":[0-9]+:");
        errorline.indexIn(error);
        QString text = errorline.capturedTexts().at(0);
        text.replace(":", "");
        if(text.toInt()-3 > 0)
            emit errored(error, text.toInt()-3);
        else
            emit errored(error, text.toInt());
        return false;
    }
    shaderProgramMutex.lock();
        if(shaderProgram)
            delete shaderProgram;
        shaderProgram = newShaderProgram;

        vertexAttr   = shaderProgram->attributeLocation("position");
        glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
        shaderProgram->setAttributeBuffer(vertexAttr, GL_FLOAT, 0, 3);
        shaderProgram->enableAttributeArray(vertexAttr);

        uvAttr       = shaderProgram->attributeLocation("texCoord");
        glBindBuffer(GL_ARRAY_BUFFER, uvBuffer);
        shaderProgram->setAttributeBuffer("texCoord", GL_FLOAT, 0, 2);
        shaderProgram->enableAttributeArray(uvAttr);

        timeUniform  = shaderProgram->uniformLocation("time");
        mouseUniform = shaderProgram->uniformLocation("mouse");
        rationUniform = shaderProgram->uniformLocation("ration");

        fragmentSource = fragmentShader;
    shaderProgramMutex.unlock();

//    qDebug() << "vertexAttr" << vertexAttr;
//    qDebug() << "uvAttr" << uvAttr;
//    qDebug() << "timeUniform" << timeUniform;
//    qDebug() << "audioUniform" << audioUniform;
    return true;
}

/**
 * @brief Renderer::render
 *
 * Initialize output device, execute the shader and display the result
 */
void Renderer::render(){
    if(!device)
        device = new QOpenGLPaintDevice();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

    device->setSize(size());

//    qDebug() << QLatin1String(reinterpret_cast<const char*>(glGetString(GL_VERSION))) << " " << QLatin1String(reinterpret_cast<const char*>(glGetString(GL_SHADING_LANGUAGE_VERSION)));
    const qreal retinaScale = devicePixelRatio();
    glViewport(0, 0, width() * retinaScale, height() * retinaScale);

    glClear(GL_COLOR_BUFFER_BIT);

    QPoint mouse = this->mapFromGlobal(QCursor::pos());
    QVector2D mousePosition((float)mouse.x() / (float)this->width(),
                            (float)mouse.y() / (float)this->height());
    float ration = (float)this->width() / (float)this->height();

    shaderProgramMutex.lock();
        vao->bind();
        shaderProgram->bind();

        shaderProgram->setUniformValue(mouseUniform,mousePosition);
        if(this->height() != 0)
            shaderProgram->setUniformValue(rationUniform, ration);
        shaderProgram->setUniformValue(timeUniform, (float)time->elapsed());

        glDrawArrays(GL_TRIANGLES, 0, 6);

        vao->release();
    shaderProgramMutex.unlock();
}

/**
 * @brief Renderer::renderLater
 *
 * Enqueue an update event to event queue
 */
void Renderer::renderLater(){
    if(!pendingUpdate){
        QCoreApplication::postEvent(this, new QEvent(QEvent::UpdateRequest));
        pendingUpdate = true;
    }
}

/**
 * @brief Renderer::renderNow
 *
 * Use the compiled shader to render on the widget
 */
void Renderer::renderNow(){
    pendingUpdate = false;

    if(!isExposed())
        return;

    bool needsInit = false;

    if(!context){
        context = new QOpenGLContext(this);
        context->setFormat(requestedFormat());
        context->create();

        needsInit = true;
    }

    context->makeCurrent(this);

    if(needsInit){
        if (m_logger->initialize()){
            m_logger->startLogging( QOpenGLDebugLogger::SynchronousLogging );
            m_logger->enableMessages();
        }
        initializeOpenGLFunctions();
        init();
    }

    if(!shaderProgram)
        initShaders(fragmentSource);

    if(shaderProgram)
        render();

    context->swapBuffers(this);

    renderLater();
}

/**
 * @brief Renderer::event
 * @param event The event that should be proccessed
 * @return True if the event was successful proccessed, otherwise false
 *
 * Called if a new event is poped from the event-queue to render on update event
 * and emit doneSignal on close event.
 */
bool Renderer::event(QEvent *event){
    switch(event->type()){
    case QEvent::UpdateRequest:
        QCoreApplication::postEvent(this, new QEvent(QEvent::UpdateRequest));
        renderNow();
        return true;
    case QEvent::Close:
        emit doneSignal(tr("User closed renderer"));
        return true;
    default:
        return QWindow::event(event);
    }
}

/**
 * @brief Renderer::exposeEvent
 *
 * Called if the window is ready to start rendering
 */
void Renderer::exposeEvent(QExposeEvent *){
    if(isExposed())
        renderNow();
}

/**
 * @brief Renderer::updateCode
 * @param filename Text for the title
 * @param code New shader program code
 * @return True on success, otherwise false.
 *
 * Set new title and compile new code for the shader program
 */
bool Renderer::updateCode(const QString &filename, const QString &code){
    if(!initShaders(code))
        return false;
    setTitle(filename);
    show();
    return true;
}

/**
 * @brief Renderer::updateAudioData
 * @param data New audio data
 *
 * Copy the new sound-data to the graphics memory for visualisation
 */
void Renderer::updateAudioData(QByteArray data){
    if(!shaderProgram)
        return;
    GLenum type, internalType;
    Q_UNUSED(internalType);
    char typeSize;
    switch(audio->format().sampleType() + audio->format().sampleSize()){
        case  8: case 10:
            type = GL_UNSIGNED_BYTE;
            internalType = GL_R8UI;
            typeSize = 1;
            break;
        case  9:
            type = GL_BYTE;
            internalType = GL_R8I;
            typeSize = 1;
            break;
        case 16: case 18:
            type = GL_UNSIGNED_SHORT;
            internalType = GL_R16UI;
            typeSize = 2;
            break;
        case 17:
            type = GL_SHORT;
            internalType = GL_R16I;
            typeSize = 2;
            break;
        case 32: case 34:
            type = GL_UNSIGNED_INT;
            internalType = GL_R32UI;
            typeSize = 4;
            break;
        case 33:
            type = GL_INT;
            internalType = GL_R32I;
            typeSize = 4;
            break;
        case 35:
            type = GL_FLOAT;
            internalType = GL_R32F;
            typeSize = 4;
            break;
        default: return;
    }
    char *left, *right;
    int count = data.size();
    if(audio->format().channelCount() == 2){
        count /= 2;
        left  = new char[count];
        right = new char[count];
        for(int i = 0; i < count; i += typeSize){
            for(int j = 0; j < typeSize; ++j){
                left [i+j] = data[i*2+j];
                right[i+j] = data[i*2+j+typeSize];
            }
        }
    }else
        left = right = data.data();

    shaderProgramMutex.lock();
    shaderProgram->bind();

    glBindTexture(GL_TEXTURE_1D, audioLeftTexture);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexImage1D(GL_TEXTURE_1D, 0, GL_R32F, count / typeSize, 0, GL_RED, type, left);

    glBindTexture(GL_TEXTURE_1D, audioRightTexture);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexImage1D(GL_TEXTURE_1D, 0, GL_R32F, count / typeSize, 0, GL_RED, type, right);

    shaderProgram->release();
    shaderProgramMutex.unlock();
    if(left != data.data())
        delete[] left;
    if(right != data.data())
        delete[] right;
}

/**
 * @brief Renderer::onMessageLogged
 * @param message Message text
 *
 * Write the message text in the debug output
 */
void Renderer::onMessageLogged(QOpenGLDebugMessage message){
    qDebug() << message;
}
<commit_msg>Highlight correct line while using mesa-driver<commit_after>#include "Renderer.hpp"

static GLfloat vertices[] = {
    1, 1,0,  1,-1,0, -1,1,0,
    1,-1,0, -1,-1,0, -1,1,0
};
static GLfloat uvs[] = {
    1,0, 1,1, 0,0,
    1,1, 0,1, 0,0
};

const char * Renderer::defaultVertexShader =
        "#version 330 core\n"
        "\n"
        "in vec3 position;\n"
        "in vec2 texCoord;\n"
        "\n"
        "out vec2  uv;\n"
        "\n"
        "void main(){\n"
        "   gl_Position = vec4(position, 1);\n"
        "   uv = texCoord;\n"
        "}";

const char * Renderer::defaultFragmentShader =
        "#version 330 core\n"
        "\n"
        "out vec4 color;\n"
        "\n"
        "in vec2 uv;\n"
        "uniform float time;\n"
        "uniform vec2 mouse;\n"
        "uniform float ration;\n"
        "\n"
        "void main(){\n"
        "     color = vec4(cos(uv.x * 5 - time / 1000) / 2 + .5, 0, sin(uv.x * 5 - time / 1000) / 2 + .5, 1);\n"
        "}";

/**
 * @brief Renderer::Renderer
 * @param parent Parent object of the render window
 *
 * Create a new Renderer with default shader
 */
Renderer::Renderer(QWindow *parent) : Renderer::Renderer("new", defaultFragmentShader, parent){ }

/**
 * @brief Renderer::Renderer
 * @param filename Name that should shown in the title
 * @param instructions Shader code for execution
 * @param parent Parent object of the render window
 *
 * Create a new Renderer with given code and set filename as title
 */
Renderer::Renderer(const QString &filename, const QString &instructions, QWindow *parent) :
    QWindow(parent),
    clearColor(Qt::black),
    context(0), device(0),
    time(0),
    pendingUpdate(false),
    vao(0), uvBuffer(0), audioLeftTexture(0), audioRightTexture(0),
    vertexAttr(0), uvAttr(0), timeUniform(0),
    shaderProgram(0),
    fragmentSource(instructions)
{
    setTitle(filename);

    m_logger = new QOpenGLDebugLogger( this );

    connect(m_logger, SIGNAL(messageLogged(QOpenGLDebugMessage)),
             this, SLOT(onMessageLogged(QOpenGLDebugMessage)),
             Qt::DirectConnection );

    setSurfaceType(QWindow::OpenGLSurface);

    time = new QTime();
    time->start();

    audio = new AudioInputProcessor(this);
    connect(audio, SIGNAL(processData(QByteArray)), this, SLOT(updateAudioData(QByteArray)));
    audio->start();

    QSurfaceFormat format;
    format.setMajorVersion(3);
    format.setMinorVersion(3);
    format.setSamples(4);
    format.setProfile(QSurfaceFormat::CoreProfile);
    setFormat(format);
}

/**
 * @brief Renderer::~Renderer
 *
 * Free resources
 */
Renderer::~Renderer(){
    glDeleteBuffers(1, &uvBuffer);
    glDeleteTextures(1, &audioLeftTexture);
    glDeleteTextures(1, &audioRightTexture);
    delete time;
    delete vao;
    delete device;
    delete m_logger;
}

/**
 * @brief Renderer::init
 * @return True on success, otherwise false
 *
 * Allocates grafic memory and initialize the shader program
 */
bool Renderer::init(){
    delete vao;
    vao = new QOpenGLVertexArrayObject(this);
    vao->create();
    vao->bind();

    glDeleteBuffers(1, &vertexBuffer);
    glGenBuffers(1, &vertexBuffer);
    glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    glDeleteBuffers(1, &uvBuffer);
    glGenBuffers(1, &uvBuffer);
    glBindBuffer(GL_ARRAY_BUFFER, uvBuffer);
    glBufferData(GL_ARRAY_BUFFER, sizeof(uvs), uvs, GL_STATIC_DRAW);

    glEnable(GL_TEXTURE_1D);

    glDeleteTextures(1, &audioLeftTexture);
    glGenTextures(1, &audioLeftTexture);
    glDeleteTextures(1, &audioRightTexture);
    glGenTextures(1, &audioRightTexture);

    glClearColor(0,0,.3,1);
    bool result = initShaders(fragmentSource);

    vao->release();

    return result;
}

/**
 * @brief Renderer::initShaders
 * @param fragmentShader Code to compile as shader
 * @return True on success, otherwise false
 *
 * Initialze and compile the shader program
 */
bool Renderer::initShaders(const QString &fragmentShader){
    QOpenGLShaderProgram *newShaderProgram = new QOpenGLShaderProgram(this);
    bool hasError = false;
    QString error = "";

    if(!hasError && !newShaderProgram->addShaderFromSourceCode(QOpenGLShader::Vertex, defaultVertexShader)){
        hasError = true;
        error = newShaderProgram->log();

        qWarning() << tr("Failed to compile default shader.");
    }
    if(!hasError && !newShaderProgram->addShaderFromSourceCode(QOpenGLShader::Fragment,fragmentShader)){
        hasError = true;
        error = newShaderProgram->log();

        if(fragmentShader == defaultFragmentShader)
            qWarning() << tr("Failed to compile default shader.");
        else if(shaderProgram == 0)
            initShaders(defaultFragmentShader);
    }
    if(!hasError && !newShaderProgram->link()){
        hasError = true;
        error = newShaderProgram->log();

        if(fragmentShader == defaultFragmentShader)
            qWarning() << tr("Failed to compile default shader.");
        else if(shaderProgram == 0)
            initShaders(defaultFragmentShader);
    }
    if(hasError){
        delete newShaderProgram;

        //mac  :<line>:
        //mesa :<line>(<errorcode>):
        QRegExp errorline(":([0-9]+)(\\([0-9]+\\))?:");
        if(errorline.indexIn(error) > -1){
            QString text = errorline.cap(1);
            if(text.toInt()-3 > 0)  // because: "#define lowp", "#define mediump" and "#define highp"
                emit errored(error, text.toInt()-3);
            else
                emit errored(error, text.toInt());
        }
        return false;
    }
    shaderProgramMutex.lock();
        if(shaderProgram)
            delete shaderProgram;
        shaderProgram = newShaderProgram;

        vertexAttr   = shaderProgram->attributeLocation("position");
        glBindBuffer(GL_ARRAY_BUFFER, vertexBuffer);
        shaderProgram->setAttributeBuffer(vertexAttr, GL_FLOAT, 0, 3);
        shaderProgram->enableAttributeArray(vertexAttr);

        uvAttr       = shaderProgram->attributeLocation("texCoord");
        glBindBuffer(GL_ARRAY_BUFFER, uvBuffer);
        shaderProgram->setAttributeBuffer("texCoord", GL_FLOAT, 0, 2);
        shaderProgram->enableAttributeArray(uvAttr);

        timeUniform  = shaderProgram->uniformLocation("time");
        mouseUniform = shaderProgram->uniformLocation("mouse");
        rationUniform = shaderProgram->uniformLocation("ration");

        fragmentSource = fragmentShader;
    shaderProgramMutex.unlock();

//    qDebug() << "vertexAttr" << vertexAttr;
//    qDebug() << "uvAttr" << uvAttr;
//    qDebug() << "timeUniform" << timeUniform;
//    qDebug() << "audioUniform" << audioUniform;
    return true;
}

/**
 * @brief Renderer::render
 *
 * Initialize output device, execute the shader and display the result
 */
void Renderer::render(){
    if(!device)
        device = new QOpenGLPaintDevice();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

    device->setSize(size());

//    qDebug() << QLatin1String(reinterpret_cast<const char*>(glGetString(GL_VERSION))) << " " << QLatin1String(reinterpret_cast<const char*>(glGetString(GL_SHADING_LANGUAGE_VERSION)));
    const qreal retinaScale = devicePixelRatio();
    glViewport(0, 0, width() * retinaScale, height() * retinaScale);

    glClear(GL_COLOR_BUFFER_BIT);

    QPoint mouse = this->mapFromGlobal(QCursor::pos());
    QVector2D mousePosition((float)mouse.x() / (float)this->width(),
                            (float)mouse.y() / (float)this->height());
    float ration = (float)this->width() / (float)this->height();

    shaderProgramMutex.lock();
        vao->bind();
        shaderProgram->bind();

        shaderProgram->setUniformValue(mouseUniform,mousePosition);
        if(this->height() != 0)
            shaderProgram->setUniformValue(rationUniform, ration);
        shaderProgram->setUniformValue(timeUniform, (float)time->elapsed());

        glDrawArrays(GL_TRIANGLES, 0, 6);

        vao->release();
    shaderProgramMutex.unlock();
}

/**
 * @brief Renderer::renderLater
 *
 * Enqueue an update event to event queue
 */
void Renderer::renderLater(){
    if(!pendingUpdate){
        QCoreApplication::postEvent(this, new QEvent(QEvent::UpdateRequest));
        pendingUpdate = true;
    }
}

/**
 * @brief Renderer::renderNow
 *
 * Use the compiled shader to render on the widget
 */
void Renderer::renderNow(){
    pendingUpdate = false;

    if(!isExposed())
        return;

    bool needsInit = false;

    if(!context){
        context = new QOpenGLContext(this);
        context->setFormat(requestedFormat());
        context->create();

        needsInit = true;
    }

    context->makeCurrent(this);

    if(needsInit){
        if (m_logger->initialize()){
            m_logger->startLogging( QOpenGLDebugLogger::SynchronousLogging );
            m_logger->enableMessages();
        }
        initializeOpenGLFunctions();
        init();
    }

    if(!shaderProgram)
        initShaders(fragmentSource);

    if(shaderProgram)
        render();

    context->swapBuffers(this);

    renderLater();
}

/**
 * @brief Renderer::event
 * @param event The event that should be proccessed
 * @return True if the event was successful proccessed, otherwise false
 *
 * Called if a new event is poped from the event-queue to render on update event
 * and emit doneSignal on close event.
 */
bool Renderer::event(QEvent *event){
    switch(event->type()){
    case QEvent::UpdateRequest:
        QCoreApplication::postEvent(this, new QEvent(QEvent::UpdateRequest));
        renderNow();
        return true;
    case QEvent::Close:
        emit doneSignal(tr("User closed renderer"));
        return true;
    default:
        return QWindow::event(event);
    }
}

/**
 * @brief Renderer::exposeEvent
 *
 * Called if the window is ready to start rendering
 */
void Renderer::exposeEvent(QExposeEvent *){
    if(isExposed())
        renderNow();
}

/**
 * @brief Renderer::updateCode
 * @param filename Text for the title
 * @param code New shader program code
 * @return True on success, otherwise false.
 *
 * Set new title and compile new code for the shader program
 */
bool Renderer::updateCode(const QString &filename, const QString &code){
    if(!initShaders(code))
        return false;
    setTitle(filename);
    show();
    return true;
}

/**
 * @brief Renderer::updateAudioData
 * @param data New audio data
 *
 * Copy the new sound-data to the graphics memory for visualisation
 */
void Renderer::updateAudioData(QByteArray data){
    if(!shaderProgram)
        return;
    GLenum type, internalType;
    Q_UNUSED(internalType);
    char typeSize;
    switch(audio->format().sampleType() + audio->format().sampleSize()){
        case  8: case 10:
            type = GL_UNSIGNED_BYTE;
            internalType = GL_R8UI;
            typeSize = 1;
            break;
        case  9:
            type = GL_BYTE;
            internalType = GL_R8I;
            typeSize = 1;
            break;
        case 16: case 18:
            type = GL_UNSIGNED_SHORT;
            internalType = GL_R16UI;
            typeSize = 2;
            break;
        case 17:
            type = GL_SHORT;
            internalType = GL_R16I;
            typeSize = 2;
            break;
        case 32: case 34:
            type = GL_UNSIGNED_INT;
            internalType = GL_R32UI;
            typeSize = 4;
            break;
        case 33:
            type = GL_INT;
            internalType = GL_R32I;
            typeSize = 4;
            break;
        case 35:
            type = GL_FLOAT;
            internalType = GL_R32F;
            typeSize = 4;
            break;
        default: return;
    }
    char *left, *right;
    int count = data.size();
    if(audio->format().channelCount() == 2){
        count /= 2;
        left  = new char[count];
        right = new char[count];
        for(int i = 0; i < count; i += typeSize){
            for(int j = 0; j < typeSize; ++j){
                left [i+j] = data[i*2+j];
                right[i+j] = data[i*2+j+typeSize];
            }
        }
    }else
        left = right = data.data();

    shaderProgramMutex.lock();
    shaderProgram->bind();

    glBindTexture(GL_TEXTURE_1D, audioLeftTexture);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexImage1D(GL_TEXTURE_1D, 0, GL_R32F, count / typeSize, 0, GL_RED, type, left);

    glBindTexture(GL_TEXTURE_1D, audioRightTexture);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
    glTexImage1D(GL_TEXTURE_1D, 0, GL_R32F, count / typeSize, 0, GL_RED, type, right);

    shaderProgram->release();
    shaderProgramMutex.unlock();
    if(left != data.data())
        delete[] left;
    if(right != data.data())
        delete[] right;
}

/**
 * @brief Renderer::onMessageLogged
 * @param message Message text
 *
 * Write the message text in the debug output
 */
void Renderer::onMessageLogged(QOpenGLDebugMessage message){
    qDebug() << message;
}
<|endoftext|>


<commit_before>#include "paxos/parliament.hpp"

Parliament::Parliament(
    Replica legislator,
    std::string location,
    DecreeHandler decree_handler)
    : signal(std::make_shared<Signal>()),
      legislator(legislator),
      legislators(LoadReplicaSet(
          std::ifstream(
              (fs::path(location) / fs::path(ReplicasetFilename)).string()))),
      receiver(std::make_shared<NetworkReceiver<BoostServer>>(
               legislator.hostname, legislator.port)),
      sender(std::make_shared<NetworkSender<BoostTransport>>(legislators)),
      bootstrap(
          std::make_shared<BootstrapListener<BoostServer>>(
              legislators,
              legislator.hostname,
              legislator.port + 1
          )
      ),
      ledger(std::make_shared<Ledger>(
          std::make_shared<PersistentQueue<Decree>>(location, "paxos.ledger"),
          decree_handler,
          DecreeHandler([this, location, legislator](std::string entry){
              SystemOperation op = Deserialize<SystemOperation>(entry);
              if (op.operation == SystemOperationType::AddReplica)
              {
                  legislators->Add(op.replica);
                  std::ofstream replicasetfile(
                      (fs::path(location) /
                       fs::path(ReplicasetFilename)).string());
                  SaveReplicaSet(legislators, replicasetfile);

                  // Only decree author sends bootstrap.
                  if (op.author.hostname == legislator.hostname &&
                      op.author.port == legislator.port)
                  {
                      SendBootstrap<BoostTransport>(
                          op.replica,
                          Deserialize<BootstrapMetadata>(op.content));
                  }
              }
              else if (op.operation == SystemOperationType::RemoveReplica)
              {
                  legislators->Remove(op.replica);
                  std::ofstream replicasetfile(
                      (fs::path(op.content) /
                       fs::path(ReplicasetFilename)).string());
                  SaveReplicaSet(legislators, replicasetfile);
              }
              signal->Set();
          }))),
      learner(std::make_shared<LearnerContext>(legislators, ledger)),
      location(location)
{
    auto acceptor = std::make_shared<AcceptorContext>(
        std::make_shared<PersistentDecree>(location, "paxos.promised_decree"),
        std::make_shared<PersistentDecree>(location, "paxos.accepted_decree"));
    hookup_legislator(legislator, acceptor);
}

Parliament::Parliament(const Parliament& other)
    : signal(other.signal),
      legislator(other.legislator),
      legislators(other.legislators),
      receiver(other.receiver),
      sender(other.sender),
      bootstrap(other.bootstrap),
      ledger(other.ledger),
      learner(other.learner),
      location(other.location)
{
}

Parliament::Parliament(
    Replica legislator,
    std::shared_ptr<ReplicaSet> legislators,
    std::shared_ptr<Ledger> ledger,
    std::shared_ptr<Receiver> receiver,
    std::shared_ptr<Sender> sender,
    std::shared_ptr<AcceptorContext> acceptor
) :
    signal(std::make_shared<Signal>()),
    legislators(legislators),
    receiver(receiver),
    sender(sender),
    ledger(ledger),
    learner(std::make_shared<LearnerContext>(legislators, ledger))
{
    signal->Set();
    hookup_legislator(legislator, acceptor);
}

void
Parliament::hookup_legislator(
    Replica replica,
    std::shared_ptr<AcceptorContext> acceptor)
{
    auto proposer = std::make_shared<ProposerContext>(
        legislators,
        ledger
    );
    auto updater = std::make_shared<UpdaterContext>(ledger);

    RegisterProposer(
        receiver,
        sender,
        proposer
    );
    RegisterAcceptor(
        receiver,
        sender,
        acceptor
    );
    RegisterLearner(
        receiver,
        sender,
        learner
    );
    RegisterUpdater(
        receiver,
        sender,
        updater
    );
}

void
Parliament::AddLegislator(
    std::string address,
    short port,
    std::string remote)
{
    Decree d;
    d.type = DecreeType::SystemDecree;
    d.content = Serialize(
        SystemOperation(
            SystemOperationType::AddReplica,
            legislator,
            0,
            Replica(address, port),
            Serialize(
                BootstrapMetadata
                {
                    location,
                    remote
                }
            )
        )
    );
    send_decree(d);
    signal->Wait();
}

void
Parliament::RemoveLegislator(
    std::string address,
    short port,
    std::string remote)
{
    Decree d;
    d.type = DecreeType::SystemDecree;
    d.content = Serialize(
        SystemOperation(
            SystemOperationType::RemoveReplica,
            legislator,
            0,
            Replica(address, port),
            Serialize(
                BootstrapMetadata
                {
                    location,
                    remote
                }
            )
        )
    );
    send_decree(d);
    signal->Wait();
}

void
Parliament::SendProposal(std::string entry)
{
    Decree d;
    d.content = entry;
    send_decree(d);
}

void
Parliament::send_decree(Decree d)
{
    Message m(
        d,
        legislator,
        legislator,
        MessageType::RequestMessage);

    sender->Reply(m);
}

AbsenteeBallots
Parliament::GetAbsenteeBallots(int max_ballots)
{
    std::map<Decree, std::shared_ptr<ReplicaSet>, compare_decree> ballots;

    int current = 0;
    for (auto vote : learner->accepted_map)
    {
        if (current <= max_ballots)
        {
            ballots[vote.first] = learner->replicaset->Difference(vote.second);
        }
        current += 1;
    }
    return ballots;

}

void
Parliament::SetActive()
{
    learner->is_observer = false;
}

void
Parliament::SetInactive()
{
    learner->is_observer = true;
}
<commit_msg>Fix remove legislator to update replicaset<commit_after>#include "paxos/parliament.hpp"

Parliament::Parliament(
    Replica legislator,
    std::string location,
    DecreeHandler decree_handler)
    : signal(std::make_shared<Signal>()),
      legislator(legislator),
      legislators(LoadReplicaSet(
          std::ifstream(
              (fs::path(location) / fs::path(ReplicasetFilename)).string()))),
      receiver(std::make_shared<NetworkReceiver<BoostServer>>(
               legislator.hostname, legislator.port)),
      sender(std::make_shared<NetworkSender<BoostTransport>>(legislators)),
      bootstrap(
          std::make_shared<BootstrapListener<BoostServer>>(
              legislators,
              legislator.hostname,
              legislator.port + 1
          )
      ),
      ledger(std::make_shared<Ledger>(
          std::make_shared<PersistentQueue<Decree>>(location, "paxos.ledger"),
          decree_handler,
          DecreeHandler([this, location, legislator](std::string entry){
              SystemOperation op = Deserialize<SystemOperation>(entry);
              if (op.operation == SystemOperationType::AddReplica)
              {
                  legislators->Add(op.replica);
                  std::ofstream replicasetfile(
                      (fs::path(location) /
                       fs::path(ReplicasetFilename)).string());
                  SaveReplicaSet(legislators, replicasetfile);

                  // Only decree author sends bootstrap.
                  if (op.author.hostname == legislator.hostname &&
                      op.author.port == legislator.port)
                  {
                      SendBootstrap<BoostTransport>(
                          op.replica,
                          Deserialize<BootstrapMetadata>(op.content));
                  }
              }
              else if (op.operation == SystemOperationType::RemoveReplica)
              {
                  legislators->Remove(op.replica);
                  std::ofstream replicasetfile(
                      (fs::path(location) /
                       fs::path(ReplicasetFilename)).string());
                  SaveReplicaSet(legislators, replicasetfile);
              }
              signal->Set();
          }))),
      learner(std::make_shared<LearnerContext>(legislators, ledger)),
      location(location)
{
    auto acceptor = std::make_shared<AcceptorContext>(
        std::make_shared<PersistentDecree>(location, "paxos.promised_decree"),
        std::make_shared<PersistentDecree>(location, "paxos.accepted_decree"));
    hookup_legislator(legislator, acceptor);
}

Parliament::Parliament(const Parliament& other)
    : signal(other.signal),
      legislator(other.legislator),
      legislators(other.legislators),
      receiver(other.receiver),
      sender(other.sender),
      bootstrap(other.bootstrap),
      ledger(other.ledger),
      learner(other.learner),
      location(other.location)
{
}

Parliament::Parliament(
    Replica legislator,
    std::shared_ptr<ReplicaSet> legislators,
    std::shared_ptr<Ledger> ledger,
    std::shared_ptr<Receiver> receiver,
    std::shared_ptr<Sender> sender,
    std::shared_ptr<AcceptorContext> acceptor
) :
    signal(std::make_shared<Signal>()),
    legislators(legislators),
    receiver(receiver),
    sender(sender),
    ledger(ledger),
    learner(std::make_shared<LearnerContext>(legislators, ledger))
{
    signal->Set();
    hookup_legislator(legislator, acceptor);
}

void
Parliament::hookup_legislator(
    Replica replica,
    std::shared_ptr<AcceptorContext> acceptor)
{
    auto proposer = std::make_shared<ProposerContext>(
        legislators,
        ledger
    );
    auto updater = std::make_shared<UpdaterContext>(ledger);

    RegisterProposer(
        receiver,
        sender,
        proposer
    );
    RegisterAcceptor(
        receiver,
        sender,
        acceptor
    );
    RegisterLearner(
        receiver,
        sender,
        learner
    );
    RegisterUpdater(
        receiver,
        sender,
        updater
    );
}

void
Parliament::AddLegislator(
    std::string address,
    short port,
    std::string remote)
{
    Decree d;
    d.type = DecreeType::SystemDecree;
    d.content = Serialize(
        SystemOperation(
            SystemOperationType::AddReplica,
            legislator,
            0,
            Replica(address, port),
            Serialize(
                BootstrapMetadata
                {
                    location,
                    remote
                }
            )
        )
    );
    send_decree(d);
    signal->Wait();
}

void
Parliament::RemoveLegislator(
    std::string address,
    short port,
    std::string remote)
{
    Decree d;
    d.type = DecreeType::SystemDecree;
    d.content = Serialize(
        SystemOperation(
            SystemOperationType::RemoveReplica,
            legislator,
            0,
            Replica(address, port),
            Serialize(
                BootstrapMetadata
                {
                    location,
                    remote
                }
            )
        )
    );
    send_decree(d);
    signal->Wait();
}

void
Parliament::SendProposal(std::string entry)
{
    Decree d;
    d.content = entry;
    send_decree(d);
}

void
Parliament::send_decree(Decree d)
{
    Message m(
        d,
        legislator,
        legislator,
        MessageType::RequestMessage);

    sender->Reply(m);
}

AbsenteeBallots
Parliament::GetAbsenteeBallots(int max_ballots)
{
    std::map<Decree, std::shared_ptr<ReplicaSet>, compare_decree> ballots;

    int current = 0;
    for (auto vote : learner->accepted_map)
    {
        if (current <= max_ballots)
        {
            ballots[vote.first] = learner->replicaset->Difference(vote.second);
        }
        current += 1;
    }
    return ballots;

}

void
Parliament::SetActive()
{
    learner->is_observer = false;
}

void
Parliament::SetInactive()
{
    learner->is_observer = true;
}
<|endoftext|>


<commit_before>// Copyright (c) 2016, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#include "hpp/pinocchio/center-of-mass-computation.hh"

#include <algorithm>
#include <boost/foreach.hpp>

#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/copy.hpp>

#include <hpp/util/exception-factory.hh>

#include "hpp/pinocchio/joint.hh"
#include <hpp/pinocchio/joint-collection.hh>
#include "hpp/pinocchio/device.hh"

namespace hpp {
  namespace pinocchio {
    CenterOfMassComputationPtr_t CenterOfMassComputation::create (
        const DevicePtr_t& d)
    {
      return CenterOfMassComputationPtr_t (new CenterOfMassComputation (d));
    }

    void CenterOfMassComputation::compute (const Computation_t& flag)
    {
      const Model& model = robot_->model();

      bool computeCOM = (flag & COM);
      bool computeJac = (flag & JACOBIAN);
      assert(computeCOM && "This does nothing");
      assert (!(computeJac && !computeCOM)); // JACOBIAN => COM

      // update kinematics
      ::pinocchio::copy(model,robot_->data(),data,0);

      data.mass[0] = 0;
      if(computeCOM) data.com[0].setZero();
      if(computeJac) data.Jcom.setZero();

      // Propagate COM initialization on all joints.
      // Could be done only on subtree, with minor gain (possibly loss because of
      // more difficult branch prediction). I dont recommend to implement it.
      for(JointIndex jid=1;jid<JointIndex(model.joints.size());++jid)
        {
          const double &            mass  = model.inertias[jid].mass ();
          data.mass[jid] = mass;
          data.com [jid] = mass*data.oMi[jid].act (model.inertias[jid].lever());
        }

      // Nullify non-subtree com and mass.
      std::size_t root = 0;
      for(std::size_t jid=1; jid<model.joints.size(); ++jid )
        {
          const std::size_t& rootId = roots_[root];
          if(jid == rootId)
            {
              jid = (std::size_t)data.lastChild[rootId];
              root ++;
            }
          else
            {
              data.mass[jid] = 0;
              data.com [jid] .setZero();
            }
        }

      // TODO as of now, it is not possible to access the template parameter
      // JointCollectionTpl of Model so we use the default one.
      typedef ::pinocchio::JacobianCenterOfMassBackwardStep<Model::Scalar,
              Model::Options, JointCollectionTpl> Pass;

      // Assume root is sorted from smallest id.
      // Nasty cast below, from (u-long) size_t to int.
      for( int root=int(roots_.size()-1); root>=0; --root )
      {
        std::size_t rootId = roots_[(std::size_t)root];

        // Backward loop on descendents of joint rootId.
        for( JointIndex jid = (JointIndex)data.lastChild[rootId];jid>=rootId;--jid )
          {
            if(computeJac)
              Pass::run(model.joints[jid],data.joints[jid],
                      Pass::ArgsType(model,data,false));
            else
              {
                assert(computeCOM);
                const JointIndex & parent = model.parents[jid];
                data.com [parent] += data.com [jid];
                data.mass[parent] += data.mass[jid];
              }

            //std::cout << data.oMi [jid] << std::endl;
            //std::cout << data.mass[jid] << std::endl;
            //std::cout << data.com [jid].transpose() << std::endl;
          } // end for jid

        // Backward loop on ancestors of joint rootId
        JointIndex jid = model.parents[rootId]; // loop variable
        rootId = (root>0) ? roots_[(std::size_t)(root-1)] : 0;      // root of previous subtree in roots_
        while (jid>rootId)                           // stop when meeting the next subtree
          {
            const JointIndex & parent = model.parents[jid];
            if(computeJac)
              Pass::run(model.joints[jid],data.joints[jid],
                      Pass::ArgsType(model,data,false));
            else
              {
                assert(computeCOM);
                data.com [parent] += data.com [jid];
                data.mass[parent] += data.mass[jid];
              }
            jid = parent;
          } // end while

      } // end for root in roots_

      if(computeCOM) data.com[0]  /= data.mass[0];
      if(computeJac) data.Jcom    /= data.mass[0];
    }

    CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
      robot_(d), roots_ (), //mass_ (0), jacobianCom_ (3, d->numberDof ())
      data(d->model())
    { assert (d->modelPtr()); }

    void CenterOfMassComputation::add (const JointPtr_t& j)
    {
      JointIndex jid = j->index();
      BOOST_FOREACH( const JointIndex rootId,  roots_ )
        {
          assert (std::size_t(rootId)<robot_->model().joints.size());
          // Assert that the new root is not in already-recorded subtrees.
          if( (jid >= rootId) && (data.lastChild[rootId] >= int(jid)) )
            // We are doing something stupid. Should we throw an error
            // or just return silently ?
            HPP_THROW(std::invalid_argument, "Joint " << j->name()
                << " (" << jid << ") is already in a subtree ["
                << rootId << ", " << data.lastChild[rootId] << "]");
        }

      roots_.push_back(jid);
    }

    CenterOfMassComputation::~CenterOfMassComputation ()
    {}
  }  //  namespace pinocchio
}  //  namespace hpp
<commit_msg>Compatibility with Pinocchio v2.1.6<commit_after>// Copyright (c) 2016, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#include "hpp/pinocchio/center-of-mass-computation.hh"

#include <algorithm>
#include <boost/foreach.hpp>

#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/copy.hpp>

#include <hpp/util/exception-factory.hh>

#include "hpp/pinocchio/joint.hh"
#include <hpp/pinocchio/joint-collection.hh>
#include "hpp/pinocchio/device.hh"

namespace hpp {
  namespace pinocchio {
    CenterOfMassComputationPtr_t CenterOfMassComputation::create (
        const DevicePtr_t& d)
    {
      return CenterOfMassComputationPtr_t (new CenterOfMassComputation (d));
    }

    void CenterOfMassComputation::compute (const Computation_t& flag)
    {
      const Model& model = robot_->model();

      bool computeCOM = (flag & COM);
      bool computeJac = (flag & JACOBIAN);
      assert(computeCOM && "This does nothing");
      assert (!(computeJac && !computeCOM)); // JACOBIAN => COM

      // update kinematics
      ::pinocchio::copy(model,robot_->data(),data,0);

      data.mass[0] = 0;
      if(computeCOM) data.com[0].setZero();
      if(computeJac) data.Jcom.setZero();

      // Propagate COM initialization on all joints.
      // Could be done only on subtree, with minor gain (possibly loss because of
      // more difficult branch prediction). I dont recommend to implement it.
      for(JointIndex jid=1;jid<JointIndex(model.joints.size());++jid)
        {
          const double &            mass  = model.inertias[jid].mass ();
          data.mass[jid] = mass;
          data.com [jid] = mass*data.oMi[jid].act (model.inertias[jid].lever());
        }

      // Nullify non-subtree com and mass.
      std::size_t root = 0;
      for(std::size_t jid=1; jid<model.joints.size(); ++jid )
        {
          const std::size_t& rootId = roots_[root];
          if(jid == rootId)
            {
              jid = (std::size_t)data.lastChild[rootId];
              root ++;
            }
          else
            {
              data.mass[jid] = 0;
              data.com [jid] .setZero();
            }
        }

      // TODO as of now, it is not possible to access the template parameter
      // JointCollectionTpl of Model so we use the default one.
      typedef ::pinocchio::JacobianCenterOfMassBackwardStep<Model::Scalar,
              Model::Options, JointCollectionTpl
#if PINOCCHIO_VERSION_AT_LEAST(2,1,6)
              ,Data::Matrix3x
#endif
                > Pass;

      // Assume root is sorted from smallest id.
      // Nasty cast below, from (u-long) size_t to int.
      for( int root=int(roots_.size()-1); root>=0; --root )
      {
        std::size_t rootId = roots_[(std::size_t)root];

        // Backward loop on descendents of joint rootId.
        for( JointIndex jid = (JointIndex)data.lastChild[rootId];jid>=rootId;--jid )
          {
            if(computeJac)
              Pass::run(model.joints[jid],data.joints[jid],
                      Pass::ArgsType(model,data,
#if PINOCCHIO_VERSION_AT_LEAST(2,1,6)
                        data.Jcom,
#endif
                        false));
            else
              {
                assert(computeCOM);
                const JointIndex & parent = model.parents[jid];
                data.com [parent] += data.com [jid];
                data.mass[parent] += data.mass[jid];
              }

            //std::cout << data.oMi [jid] << std::endl;
            //std::cout << data.mass[jid] << std::endl;
            //std::cout << data.com [jid].transpose() << std::endl;
          } // end for jid

        // Backward loop on ancestors of joint rootId
        JointIndex jid = model.parents[rootId]; // loop variable
        rootId = (root>0) ? roots_[(std::size_t)(root-1)] : 0;      // root of previous subtree in roots_
        while (jid>rootId)                           // stop when meeting the next subtree
          {
            const JointIndex & parent = model.parents[jid];
            if(computeJac)
              Pass::run(model.joints[jid],data.joints[jid],
                      Pass::ArgsType(model,data,
#if PINOCCHIO_VERSION_AT_LEAST(2,1,6)
                        data.Jcom,
#endif
                        false));
            else
              {
                assert(computeCOM);
                data.com [parent] += data.com [jid];
                data.mass[parent] += data.mass[jid];
              }
            jid = parent;
          } // end while

      } // end for root in roots_

      if(computeCOM) data.com[0]  /= data.mass[0];
      if(computeJac) data.Jcom    /= data.mass[0];
    }

    CenterOfMassComputation::CenterOfMassComputation (const DevicePtr_t& d) :
      robot_(d), roots_ (), //mass_ (0), jacobianCom_ (3, d->numberDof ())
      data(d->model())
    { assert (d->modelPtr()); }

    void CenterOfMassComputation::add (const JointPtr_t& j)
    {
      JointIndex jid = j->index();
      BOOST_FOREACH( const JointIndex rootId,  roots_ )
        {
          assert (std::size_t(rootId)<robot_->model().joints.size());
          // Assert that the new root is not in already-recorded subtrees.
          if( (jid >= rootId) && (data.lastChild[rootId] >= int(jid)) )
            // We are doing something stupid. Should we throw an error
            // or just return silently ?
            HPP_THROW(std::invalid_argument, "Joint " << j->name()
                << " (" << jid << ") is already in a subtree ["
                << rootId << ", " << data.lastChild[rootId] << "]");
        }

      roots_.push_back(jid);
    }

    CenterOfMassComputation::~CenterOfMassComputation ()
    {}
  }  //  namespace pinocchio
}  //  namespace hpp
<|endoftext|>


<commit_before>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepRequestID(getMotionStatus().stepControl.stepRequestID + 1),      // WalkRequest stepRequestID starts at 0, we have to start at 1
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_to_ball", "Walks to the ball from far.", false);

  getDebugParameterList().add(&params);
}

PathPlanner::~PathPlanner()
{
  getDebugParameterList().remove(&params);
}

void PathPlanner::execute()
{
  // --- DEBUG REQUESTS ---
  DEBUG_REQUEST("PathPlanner:walk_to_ball",
    if (getBallModel().positionPreview.x > 250) {
      walk_to_ball(Foot::NONE);
    }
  );
  // --- DEBUG REQUESTS ---

  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  // HACK: xabsl set a firced motion request => clear everything
  if(getPathModel().path_routine == PathModel::PathRoutine::NONE && getMotionRequest().forced) {
    step_buffer.clear();
    return;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  double ballRadius                  = getFieldInfo().ballRadius;
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance - ballRadius), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = Math::fromDegrees(0.0);

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = Math::fromDegrees(0.0);
    add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  //if (ballPos.x < getPathModel().distance + 30 && ballPos.x > getPathModel().distance - 30)
  if (stepX < 0 && ballPos.x > getPathModel().distance + 30)
  {
    stepX = 0;
  }

  const double slow_down_factor = 0.7;
  Pose2D pose;
  if (   params.approach_ball_adapt_control
      && Vector2d(stepX, stepY).abs() < params.approach_ball_adapt_threshold)
  {
    pose = { stepRotation, stepX, stepY };
  }
  else
  {
    pose = { stepRotation, slow_down_factor * stepX, slow_down_factor * stepY };
  }

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = Math::fromDegrees(0.0);
    add_step(pose, type, coordinate, character, Foot::NONE, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = Math::fromDegrees(0.0);
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = Math::fromDegrees(0.0);
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = Math::fromDegrees(0.0);
    double stepY                       = 0.0;
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate     = WalkRequest::LFoot;
      speed_direction = Math::fromDegrees(90);
      stepY          = 100;
      break;
    case Foot::RIGHT:
      coordinate     = WalkRequest::RFoot;
      speed_direction = Math::fromDegrees(-90);
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = params.sidekick_scale;

      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection, const WalkRequest::StepControlRequest::RestrictionMode restriction, bool isProtected)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? params.kick_time : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate,
                                              restriction,
                                              isProtected}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  step_buffer.front().pose = pose;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // requested step has been accepted
  if (last_stepRequestID == getMotionStatus().stepControl.stepRequestID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepRequestID = getMotionStatus().stepControl.stepRequestID + 1;
  }
}

void PathPlanner::execute_step_buffer()
{
  STOPWATCH_START("PathPlanner:execute_steplist");

  if (step_buffer.empty()) {
    return;
  }

  getMotionRequest().id                                        = motion::walk;
  getMotionRequest().walkRequest.coordinate                    = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                     = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale             = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID            = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type              = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time              = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection    = Math::fromDegrees(step_buffer.front().speedDirection);
  getMotionRequest().walkRequest.stepControl.target            = step_buffer.front().pose;
  getMotionRequest().walkRequest.stepControl.restriction       = step_buffer.front().restriction;
  getMotionRequest().walkRequest.stepControl.isProtected       = step_buffer.front().isProtected;
  getMotionRequest().walkRequest.stepControl.stepRequestID     = last_stepRequestID;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = (foot_to_use != Foot::RIGHT);
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<commit_msg>should have been commited<commit_after>/**
 * @file PathPlanner.cpp
 *
 * @author Yigit Can Akcay
 * Implementation of class PathPlanner
 */

#include "PathPlanner.h"

PathPlanner::PathPlanner()
:
step_buffer({}),
foot_to_use(Foot::RIGHT),
last_stepRequestID(getMotionStatus().stepControl.stepRequestID + 1),      // WalkRequest stepRequestID starts at 0, we have to start at 1
kick_planned(false)
{
  DEBUG_REQUEST_REGISTER("PathPlanner:walk_to_ball", "Walks to the ball from far.", false);

  getDebugParameterList().add(&params);
}

PathPlanner::~PathPlanner()
{
  getDebugParameterList().remove(&params);
}

void PathPlanner::execute()
{
  // --- DEBUG REQUESTS ---
  DEBUG_REQUEST("PathPlanner:walk_to_ball",
    if (getBallModel().positionPreview.x > 250) {
      walk_to_ball(Foot::NONE);
    }
  );
  // --- DEBUG REQUESTS ---

  getPathModel().kick_executed = false;

  STOPWATCH_START("PathPlanner");
  // Always executed first
  manage_step_buffer();

  // The kick has been executed
  // Tells XABSL to jump into next state
  if (kick_planned && step_buffer.empty()) {
    getPathModel().kick_executed = true;
  }

  // HACK: xabsl set a firced motion request => clear everything
  if(getPathModel().path_routine == PathModel::PathRoutine::NONE && getMotionRequest().forced) {
    step_buffer.clear();
    return;
  }

  switch (getPathModel().path_routine)
  {
  case PathModel::PathRoutine::NONE:
    // There is no kick planned, since the kick has been executed
    // and XABSL is in a different state now
    if (kick_planned) {
      kick_planned = false;
    }

    if (step_buffer.empty()) {
      return;
    }
    break;
  case PathModel::PathRoutine::GO_TO_BALL_FAST:
    walk_to_ball(Foot::NONE , true);
    break;
  case PathModel::PathRoutine::GO_TO_BALL_SLOW:
    walk_to_ball(Foot::NONE);
    break;
  case PathModel::PathRoutine::MOVE_AROUND_BALL:
    move_around_ball(getPathModel().direction, getPathModel().radius);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_LEFT:
    approach_ball(Foot::LEFT);
    break;
  case PathModel::PathRoutine::APPROACH_BALL_RIGHT:
    approach_ball(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_LEFT:
    short_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SHORT_KICK_RIGHT:
    short_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::LONG_KICK_LEFT:
    long_kick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::LONG_KICK_RIGHT:
    long_kick(Foot::RIGHT);
    break;
  case PathModel::PathRoutine::SIDEKICK_LEFT:
    sidekick(Foot::LEFT);
    break;
  case PathModel::PathRoutine::SIDEKICK_RIGHT:
    sidekick(Foot::RIGHT);
    break;
  }

  // Always executed last
  execute_step_buffer();

  STOPWATCH_STOP("PathPlanner");
}

void PathPlanner::walk_to_ball(const Foot foot, const bool go_fast)
{
  Vector2d ballPos                   = Vector2d();
  double ballRadius                  = getFieldInfo().ballRadius;
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  switch (foot) {
    case Foot::LEFT:
      ballPos    = getBallModel().positionPreviewInLFoot;
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::RIGHT:
      ballPos    = getBallModel().positionPreviewInRFoot;
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::NONE:
      ballPos    = getBallModel().positionPreview;
      coordinate = WalkRequest::Hip;
      break;
  }
  double ballRotation = ballPos.angle();

  Pose2D pose           = { ballRotation, 0.7*(ballPos.x - getPathModel().distance - ballRadius), ballPos.y };

  if (step_buffer.empty())
  {
    StepType type         = StepType::WALKSTEP;
    double scale          = 1.0;
    double speed_direction = Math::fromDegrees(0.0);

    if (go_fast)
    {
      double character = 1.0;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
    else
    {
      double character = 0.3;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
    }
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::move_around_ball(const double direction, const double radius)
{
  Vector2d ballPos                   = getBallModel().positionPreview;
  double ballRotation                = ballPos.angle();
  double ballDistance                = ballPos.abs();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;

  double min1;
  double min2;
  double max1;
  double max2;
  if (direction <= 0)
  {
    min1 = 0.0;
    min2 = 0.0;
    max1 = 45.0;
    max2 = 100.0;
  }
  else {
    min1 = -45;
    min2 = -100;
    max1 = 0;
    max2 = 0;
  }

  double stepX = (ballDistance - radius) * std::cos(ballRotation);
  double stepY = Math::clamp(radius * std::tan(Math::clamp(Math::toDegrees(-direction), min1, max1)), min2, max2) * std::cos(ballRotation);

  Pose2D pose           = { ballRotation, stepX, stepY };

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    Foot foot              = Foot::NONE;
    double scale           = 1.0;
    double speed_direction = Math::fromDegrees(0.0);
    add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::approach_ball(const Foot foot)
{
  Vector2d ballPos                   = Vector2d();
  WalkRequest::Coordinate coordinate = WalkRequest::Hip;
  double stepX                       = 0.0;
  double stepY                       = 0.0;
  double ballRadius                  = getFieldInfo().ballRadius;
  double stepRotation                = ballPos.abs() > 250 ? ballPos.angle() : 0;

  switch (foot) {
  case Foot::LEFT:
    ballPos    = getBallModel().positionPreviewInLFoot;
    coordinate = WalkRequest::LFoot;
    stepX      = ballPos.x - std::abs(ballPos.y - getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y - getPathModel().yOffset;
    break;
  case Foot::RIGHT:
    ballPos    = getBallModel().positionPreviewInRFoot;
    coordinate = WalkRequest::RFoot;
    stepX      = ballPos.x - std::abs(ballPos.y + getPathModel().yOffset) - getPathModel().distance - ballRadius;
    stepY      = ballPos.y + getPathModel().yOffset;
    break;
  case Foot::NONE:
    ASSERT(false);
  }

  //if (ballPos.x < getPathModel().distance + 30 && ballPos.x > getPathModel().distance - 30)
  if (stepX < 0 && ballPos.x > getPathModel().distance + 30)
  {
    stepX = 0;
  }

  const double slow_down_factor = 0.7;
  Pose2D pose;
  if (   params.approach_ball_adapt_control
      && Vector2d(stepX, stepY).abs() < params.approach_ball_adapt_threshold)
  {
    pose = { stepRotation, stepX, stepY };
  }
  else
  {
    pose = { stepRotation, slow_down_factor * stepX, slow_down_factor * stepY };
  }

  if (step_buffer.empty())
  {
    StepType type          = StepType::WALKSTEP;
    double character       = 0.7;
    double scale           = 1.0;
    double speed_direction = Math::fromDegrees(0.0);
    add_step(pose, type, coordinate, character, Foot::NONE, scale, speed_direction, WalkRequest::StepControlRequest::HARD, false);
  }
  else
  {
    update_step(pose);
  }
}

void PathPlanner::short_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500 , 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = Math::fromDegrees(0.0);
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::long_kick(const Foot foot)
{
  if (!kick_planned)
  {
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate = WalkRequest::RFoot;
      break;
    case Foot::RIGHT:
      coordinate = WalkRequest::LFoot;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose            = { 0.0, 500, 0.0 };
      StepType type          = StepType::KICKSTEP;
      double character       = 1.0;
      double scale           = 0.7;
      double speed_direction = Math::fromDegrees(0.0);
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

void PathPlanner::sidekick(const Foot foot)
{
  if (!kick_planned)
  {
    double speed_direction             = Math::fromDegrees(0.0);
    double stepY                       = 0.0;
    WalkRequest::Coordinate coordinate = WalkRequest::Hip;

    switch (foot) {
    case Foot::LEFT:
      coordinate     = WalkRequest::LFoot;
      speed_direction = Math::fromDegrees(90);
      stepY          = 100;
      break;
    case Foot::RIGHT:
      coordinate     = WalkRequest::RFoot;
      speed_direction = Math::fromDegrees(-90);
      stepY          = -100;
      break;
    case Foot::NONE:
      ASSERT(false);
    }

    if (step_buffer.empty())
    {
      Pose2D pose = { 0.0, 500, stepY };
      StepType type = StepType::KICKSTEP;
      double character = 1.0;
      Foot step_foot = foot == Foot::RIGHT ? Foot::LEFT : Foot::RIGHT;
      double scale = params.sidekick_scale;

      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      type = StepType::ZEROSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::SOFT, true);

      pose = { 0.0, 0.0, 0.0 };
      type = StepType::WALKSTEP;
      add_step(pose, type, coordinate, character, step_foot, scale, speed_direction, WalkRequest::StepControlRequest::HARD, true);

      kick_planned = true;
    }
  }
}

// Stepcontrol
void PathPlanner::add_step(Pose2D &pose, const StepType &type, const WalkRequest::Coordinate &coordinate, const double character, const Foot foot, const double scale, const double speedDirection, const WalkRequest::StepControlRequest::RestrictionMode restriction, bool isProtected)
{
  step_buffer.push_back(Step_Buffer_Element({ pose,
                                              speedDirection,
                                              type,
                                              type == StepType::KICKSTEP ? params.kick_time : 250,
                                              character,
                                              scale,
                                              foot,
                                              coordinate,
                                              restriction,
                                              isProtected}));
}
void PathPlanner::update_step(Pose2D &pose)
{
  ASSERT(step_buffer.size() > 0);

  step_buffer.front().pose = pose;
}

void PathPlanner::manage_step_buffer()
{
  if (step_buffer.empty()) {
    return;
  }

  // requested step has been accepted
  if (last_stepRequestID == getMotionStatus().stepControl.stepRequestID)
  {
    step_buffer.erase(step_buffer.begin());
    last_stepRequestID = getMotionStatus().stepControl.stepRequestID + 1;
  }
}

void PathPlanner::execute_step_buffer()
{
  STOPWATCH_START("PathPlanner:execute_steplist");

  if (step_buffer.empty()) {
    return;
  }

  getMotionRequest().id                                        = motion::walk;
  getMotionRequest().walkRequest.coordinate                    = step_buffer.front().coordinate;
  getMotionRequest().walkRequest.character                     = step_buffer.front().character;
  getMotionRequest().walkRequest.stepControl.scale             = step_buffer.front().scale;
  getMotionRequest().walkRequest.stepControl.stepID            = getMotionStatus().stepControl.stepID;
  getMotionRequest().walkRequest.stepControl.type              = step_buffer.front().type;
  getMotionRequest().walkRequest.stepControl.time              = step_buffer.front().time;
  getMotionRequest().walkRequest.stepControl.speedDirection    = step_buffer.front().speedDirection;
  getMotionRequest().walkRequest.stepControl.target            = step_buffer.front().pose;
  getMotionRequest().walkRequest.stepControl.restriction       = step_buffer.front().restriction;
  getMotionRequest().walkRequest.stepControl.isProtected       = step_buffer.front().isProtected;
  getMotionRequest().walkRequest.stepControl.stepRequestID     = last_stepRequestID;

  // normal walking WALKSTEPs use Foot::NONE, for KICKSTEPs the foot to use has to be specified
  if (step_buffer.front().foot == Foot::NONE)
  {
    switch (getMotionStatus().stepControl.moveableFoot)
    {
    case MotionStatus::StepControlStatus::LEFT:
      foot_to_use = Foot::LEFT;
      break;
    case MotionStatus::StepControlStatus::RIGHT:
      foot_to_use = Foot::RIGHT;
      break;
    case MotionStatus::StepControlStatus::BOTH:
      if (step_buffer.front().pose.translation.y > 0.0f || step_buffer.front().pose.rotation > 0.0f)
      {
        foot_to_use = Foot::LEFT;
      }
      else
      {
        foot_to_use = Foot::RIGHT;
      }
      break;
    case MotionStatus::StepControlStatus::NONE:
      foot_to_use = Foot::RIGHT;
      break;
    }
  }
  else
  {
    foot_to_use = step_buffer.front().foot;
  }
  // false means right foot
  getMotionRequest().walkRequest.stepControl.moveLeftFoot = (foot_to_use != Foot::RIGHT);
  STOPWATCH_STOP("PathPlanner:execute_steplist");
}
<|endoftext|>