<commit_before>// *********************************************************
// Filename: Question.cpp
// Description:
//
// Author: Eliasar Gandara
// Created On:
// Last Modified:
// *********************************************************

#include "Question.h"
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <cmath>
#include <ctype.h>
#include <string>
using namespace std;

Question::Question()
{
    srand(time(NULL));
    file_name = "file.txt";

    num_past_questions = 0;

    clearAnswers();
    createNewQuestion();
}

int Question::stringToInt(string str)
{
    int total = 0;
    int value = 0;
    bool isDigit = true;

    for (unsigned int i = 0; i < str.length(); i++)
    {
        if (!isdigit(str[0]))
        {
            isDigit = false;
        }
    }

    if (isDigit)
    {
        for (unsigned int i = 0; i < str.length(); i++)
        {
            value = (str[i] - 48);
            total += value * pow(10, str.length() - 1 - i);
        }
    }
    else
        total = -1;

    return total;
}

void Question::displayAnswers()
{
    for (int row = 0; row < 8; row++)
    {
        for (int col = 0; col < 4; col++)
        {
            cout << answers[row][col] << ", ";
        }
        cout << endl;
    }
}

void Question::createNewQuestion()
{
    ifstream in;
    string input;
    int current_question = 0;

    do
    {
        question_number = (rand() % 20) + 1;
    } while (inPastQuestions());
    num_past_questions++;

    in.open(file_name.c_str());

    if(in.fail())       // Check to make sure the file opens properly
    {
        cout << "Error opening file " << file_name << endl;     // Will display message and exit program if file opening fails
        exit(1);
    }

    clearAnswers();

    while (!in.eof() && current_question != question_number + 1)
    {
        getline(in, input, ',');        // Will get the first character of a line from a file
        while (input[0] == '\n')
        {
            input = input.substr(1, input.length());
        }

        if (input[0] == '?')
        {
            current_question = stringToInt(input.substr(1,input.length()));

        }

        if (current_question == question_number)
        {
            getline(in, question, ',');

            int row = 0;
            int col = 0;
            while (!in.eof() && current_question != question_number + 1)
            {
                getline(in, input, ',');
                while (input[0] == '\n')
                {
                    input = input.substr(1, input.length());
                }

                if (input[0] == '?')
                {
                    current_question = stringToInt(input.substr(1,input.length()));

                }

                if (isdigit(input[0]))
                {
                    answers[row][3] = input;
                    row++;
                    col = 0;
                }
                else if (input[0] != '?')
                {
                    answers[row][col] = input;
                    col++;
                }

            }
        }
    }

    in.close();     // Closes file
}

int Question::checkAnswers(string answer)     // returns row of answer if answer matches the possible answers, -1 otherwise
{
    for (int row = 0; row < 8; row++)
    {
        for (int col = 0; col < 3; col++)
        {
            if (Question::tolowercase(answer) == Question::tolowercase(answers[row][col]))
            {
                return row;
            }
        }
    }

    return -1;
}

int Question::getScore(string answer)
{
    if (Question::checkAnswers(answer) != -1)
        return Question::stringToInt(answers[Question::checkAnswers(answer)][3]);
    else
        return 0;
}

string Question::tolowercase(string word)
{
    for (unsigned int i = 0; i < word.length(); i++)
    {
        word[i] = tolower(word[i]);
    }

    return word;
}

void Question::getAnswers(string arr[])
{
    for (int i = 0; i < 8; i++)
    {
        if (answers[i][0] != "")
            arr[i] = answers[i][0];
    }
}

void Question::displayQuestions()
{

    ifstream in;
    string input;

    in.open(file_name.c_str());

    if(in.fail())       // Check to make sure the file opens properly
    {
        cout << "Error opening file " << file_name << endl;     // Will display message and exit program if file opening fails
        exit(1);
    }

    // For testing purposes
    int value = 1;
    int current_question = 0;
    question_number = value;
    // Ending testing code

    clearAnswers();

    while (!in.eof() && current_question != question_number + 1)
    {
        getline(in, input, ',');        // Will get the first character of a line from a file
        while (input[0] == '\n')
        {
            input = input.substr(1, input.length());
        }
        //cout << "Input::" << input << "::Input" << endl;

        if (input[0] == '?')
        {
            current_question = stringToInt(input.substr(1,input.length()));

        }

        if (current_question == question_number)
        {
            getline(in, question, ',');

            int row = 0;
            int col = 0;
            while (!in.eof() && current_question != question_number + 1)
            {
                getline(in, input, ',');
                while (input[0] == '\n')
                {
                    input = input.substr(1, input.length());
                }

                if (input[0] == '?')
                {
                    current_question = stringToInt(input.substr(1,input.length()));

                }

                if (isdigit(input[0]))
                {
                    answers[row][3] = input;
                    row++;
                    col = 0;
                }
                else if (input[0] != '?')
                {
                    answers[row][col] = input;
                    col++;
                }

            }
        }

        //cout << "Input:" << input << ":End" << endl;

    }
        //cout << "Input::" << input << "::Input" << endl;
        cout << question_number << ". " << "Question: " << question << endl;
        displayAnswers();
        cout << endl;

    in.close();     // Closes file
}

bool Question::inPastQuestions()
{
    bool isInPastQuestions = false;

    for (int i = 0; i < num_past_questions; i++)
    {
        if (question_number == past_questions[i])
            isInPastQuestions = true;
    }

    return isInPastQuestions;
}

void Question::clearAnswers()
{
    for (int i = 0; i < 8; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            answers[i][j] = "";
        }
    }
}
<commit_msg>No questions will be repeated<commit_after>// *********************************************************
// Filename: Question.cpp
// Description:
//
// Author: Eliasar Gandara
// Created On:
// Last Modified:
// *********************************************************

#include "Question.h"
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <cmath>
#include <ctype.h>
#include <string>
using namespace std;

Question::Question()
{
    srand(time(NULL));
    file_name = "file.txt";

    num_past_questions = 0;

    clearAnswers();
    createNewQuestion();
}

int Question::stringToInt(string str)
{
    int total = 0;
    int value = 0;
    bool isDigit = true;

    for (unsigned int i = 0; i < str.length(); i++)
    {
        if (!isdigit(str[0]))
        {
            isDigit = false;
        }
    }

    if (isDigit)
    {
        for (unsigned int i = 0; i < str.length(); i++)
        {
            value = (str[i] - 48);
            total += value * pow(10, str.length() - 1 - i);
        }
    }
    else
        total = -1;

    return total;
}

void Question::displayAnswers()
{
    for (int row = 0; row < 8; row++)
    {
        for (int col = 0; col < 4; col++)
        {
            cout << answers[row][col] << ", ";
        }
        cout << endl;
    }
}

void Question::createNewQuestion()
{
    ifstream in;
    string input;
    int current_question = 0;

    do
    {
        question_number = (rand() % 20) + 1;
    } while (inPastQuestions());
    past_questions[num_past_questions] = question_number;
    num_past_questions++;

    in.open(file_name.c_str());

    if(in.fail())       // Check to make sure the file opens properly
    {
        cout << "Error opening file " << file_name << endl;     // Will display message and exit program if file opening fails
        exit(1);
    }

    clearAnswers();

    while (!in.eof() && current_question != question_number + 1)
    {
        getline(in, input, ',');        // Will get the first character of a line from a file
        while (input[0] == '\n')
        {
            input = input.substr(1, input.length());
        }

        if (input[0] == '?')
        {
            current_question = stringToInt(input.substr(1,input.length()));

        }

        if (current_question == question_number)
        {
            getline(in, question, ',');

            int row = 0;
            int col = 0;
            while (!in.eof() && current_question != question_number + 1)
            {
                getline(in, input, ',');
                while (input[0] == '\n')
                {
                    input = input.substr(1, input.length());
                }

                if (input[0] == '?')
                {
                    current_question = stringToInt(input.substr(1,input.length()));

                }

                if (isdigit(input[0]))
                {
                    answers[row][3] = input;
                    row++;
                    col = 0;
                }
                else if (input[0] != '?')
                {
                    answers[row][col] = input;
                    col++;
                }

            }
        }
    }

    in.close();     // Closes file
}

int Question::checkAnswers(string answer)     // returns row of answer if answer matches the possible answers, -1 otherwise
{
    for (int row = 0; row < 8; row++)
    {
        for (int col = 0; col < 3; col++)
        {
            if (Question::tolowercase(answer) == Question::tolowercase(answers[row][col]))
            {
                return row;
            }
        }
    }

    return -1;
}

int Question::getScore(string answer)
{
    if (Question::checkAnswers(answer) != -1)
        return Question::stringToInt(answers[Question::checkAnswers(answer)][3]);
    else
        return 0;
}

string Question::tolowercase(string word)
{
    for (unsigned int i = 0; i < word.length(); i++)
    {
        word[i] = tolower(word[i]);
    }

    return word;
}

void Question::getAnswers(string arr[])
{
    for (int i = 0; i < 8; i++)
    {
        if (answers[i][0] != "")
            arr[i] = answers[i][0];
    }
}

void Question::displayQuestions()
{

    ifstream in;
    string input;

    in.open(file_name.c_str());

    if(in.fail())       // Check to make sure the file opens properly
    {
        cout << "Error opening file " << file_name << endl;     // Will display message and exit program if file opening fails
        exit(1);
    }

    // For testing purposes
    int value = 1;
    int current_question = 0;
    question_number = value;
    // Ending testing code

    clearAnswers();

    while (!in.eof() && current_question != question_number + 1)
    {
        getline(in, input, ',');        // Will get the first character of a line from a file
        while (input[0] == '\n')
        {
            input = input.substr(1, input.length());
        }
        //cout << "Input::" << input << "::Input" << endl;

        if (input[0] == '?')
        {
            current_question = stringToInt(input.substr(1,input.length()));

        }

        if (current_question == question_number)
        {
            getline(in, question, ',');

            int row = 0;
            int col = 0;
            while (!in.eof() && current_question != question_number + 1)
            {
                getline(in, input, ',');
                while (input[0] == '\n')
                {
                    input = input.substr(1, input.length());
                }

                if (input[0] == '?')
                {
                    current_question = stringToInt(input.substr(1,input.length()));

                }

                if (isdigit(input[0]))
                {
                    answers[row][3] = input;
                    row++;
                    col = 0;
                }
                else if (input[0] != '?')
                {
                    answers[row][col] = input;
                    col++;
                }

            }
        }

        //cout << "Input:" << input << ":End" << endl;

    }
        //cout << "Input::" << input << "::Input" << endl;
        cout << question_number << ". " << "Question: " << question << endl;
        displayAnswers();
        cout << endl;

    in.close();     // Closes file
}

bool Question::inPastQuestions()
{
    bool isInPastQuestions = false;

    for (int i = 0; i < num_past_questions; i++)
    {
        if (question_number == past_questions[i])
            isInPastQuestions = true;
    }

    return isInPastQuestions;
}

void Question::clearAnswers()
{
    for (int i = 0; i < 8; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            answers[i][j] = "";
        }
    }
}
<|endoftext|>


<commit_before>// FlangerApp.cpp
// e.moon 16jun99

#include "MediaNodeControlApp.h"
#include <cstdio>
#include <cstring>
#include <cstdlib>

const char* const 	g_pAppSignature = "application/x-vnd.moon-Flanger.media_addon";

int main(int argc, char** argv) {
	if(argc < 2 || strstr(argv[1], "node=") != argv[1]) {
		fprintf(stderr, "Sorry, not a standalone application.\n");
	}

	// start node control panel
	media_node_id id = atol(argv[1] + 5);
	MediaNodeControlApp app(g_pAppSignature, id);
	app.Run();

	return 0;
}

// END -- FlangerApp.cpp --<commit_msg>flanger now bails out properly on attempts to run standalone<commit_after>// FlangerApp.cpp
// e.moon 16jun99

#include "MediaNodeControlApp.h"
#include <cstdio>
#include <cstring>
#include <cstdlib>

const char* const 	g_pAppSignature = "application/x-vnd.moon-Flanger.media_addon";

int main(int argc, char** argv) {
	if(argc < 2 || strstr(argv[1], "node=") != argv[1]) {
		fprintf(stderr, "Sorry, not a standalone application.\n");
		return -1;
	}

	// start node control panel
	media_node_id id = atol(argv[1] + 5);
	MediaNodeControlApp app(g_pAppSignature, id);
	app.Run();

	return 0;
}

// END -- FlangerApp.cpp --<|endoftext|>


<commit_before>// This file is part of BlueSky
//
// BlueSky 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.
//
// BlueSky is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with BlueSky; if not, see <http://www.gnu.org/licenses/>.

#include <bs/common.h>
#include <bs/kernel.h>
#include <bs/detail/lib_descriptor.h>
#include <bs/log.h>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/stringize.hpp>

#include <string>
#include <exception>

//! define TARGET_NAME in compiler equal to output loader's name without extension
#ifndef TARGET_NAME
#define TARGET_NAME bs
#endif

#define S_TARGET_NAME BOOST_PP_STRINGIZE(TARGET_NAME)
#define INIT_FN_NAME BOOST_PP_CAT(init, TARGET_NAME)

using namespace blue_sky;
using namespace std;

BS_C_API_PLUGIN void INIT_FN_NAME() {
	//search for BlueSky's kernel plugin descriptor
	BS_GET_PLUGIN_DESCRIPTOR get_pd_fn;
	if(detail::lib_descriptor::load_sym_glob("bs_get_plugin_descriptor", get_pd_fn) != 0 || !get_pd_fn) {
		BSERROR << log::E("BlueSky kernel descriptor wasn't found or invalid!") << log::end;
		return;
		//throw std::runtime_error("BlueSky kernel descriptor wasn't found");
	}
	plugin_descriptor* kernel_pd = get_pd_fn();
	//correct global namespace to match TARGET_NAME (otherwise Python throws an error)
	kernel_pd->py_namespace = S_TARGET_NAME;

	//load plugins with Python subsystem
	//give_kernel::Instance().LoadPlugins(true);
}

<commit_msg>bspy_loader: use pybind11<commit_after>// This file is part of BlueSky
//
// BlueSky 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.
//
// BlueSky is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with BlueSky; if not, see <http://www.gnu.org/licenses/>.

#include <pybind11/pybind11.h>

#include <bs/common.h>
#include <bs/kernel.h>
#include <bs/detail/lib_descriptor.h>
#include <bs/log.h>

#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/stringize.hpp>

#include <string>
#include <exception>

//! define TARGET_NAME in compiler equal to output loader's name without extension
#ifndef TARGET_NAME
#define TARGET_NAME bs
#endif

#define S_TARGET_NAME BOOST_PP_STRINGIZE(TARGET_NAME)
#define INIT_FN_NAME BOOST_PP_CAT(init, TARGET_NAME)

using namespace blue_sky;
using namespace std;
namespace py = pybind11;

PYBIND11_PLUGIN(TARGET_NAME) {
	auto m = py::module(S_TARGET_NAME);

	//search for BlueSky's kernel plugin descriptor
	BS_GET_PLUGIN_DESCRIPTOR get_pd_fn;
	if(detail::lib_descriptor::load_sym_glob("bs_get_plugin_descriptor", get_pd_fn) != 0 || !get_pd_fn) {
		BSERROR << log::E("BlueSky kernel descriptor wasn't found or invalid!") << log::end;
		return m.ptr();
		//throw std::runtime_error("BlueSky kernel descriptor wasn't found");
	}
	plugin_descriptor* kernel_pd = get_pd_fn();
	//correct global namespace to match TARGET_NAME (otherwise Python throws an error)
	kernel_pd->py_namespace = S_TARGET_NAME;

	//load plugins with Python subsystem
	//give_kernel::Instance().LoadPlugins(true);

	return m.ptr();
}

<|endoftext|>


<commit_before>#include "consoleui.h"

using namespace std;

ConsoleUI::ConsoleUI()
{
}

void ConsoleUI::run()
{
    cout << "This is a test\n";
}
<commit_msg>Added text output in ConsoleUI<commit_after>#include "consoleui.h"

using namespace std;

ConsoleUI::ConsoleUI()
{
}

void ConsoleUI::run()
{
    cout << "Choose a command:\n";
    cout << endl;
    cout << "register\t- Register a scientist\n";
    cout << "list\t\t- List scientists\n";
    cout << "save\t\t- Save changes\n";
    cout << "quit\t\t- Exit program\n";
    cout << endl;
}
<|endoftext|>


<commit_before>/**
* @file IKParameters.cpp
*
* @author Xu, Yuan
* Implement of parameters for IK motion
*/

#include "IKParameters.h"

IKParameters::IKParameters()
:ParameterList("IKParameters")
{
  PARAMETER_REGISTER(footOffsetY) = 0;

  // stand parameter
  PARAMETER_REGISTER(stand.speed) = 0.04;
  PARAMETER_REGISTER(stand.enableStabilization) = true;
  PARAMETER_REGISTER(stand.enableStabilizationRC16) = false;
  PARAMETER_REGISTER(stand.stiffness) = 0.7;

  PARAMETER_ANGLE_REGISTER(stand.bodyPitchOffset) = 0.2;
  PARAMETER_REGISTER(stand.hipOffsetX) = 15;

  // relax
  PARAMETER_REGISTER(stand.relax.enable) = true;
  PARAMETER_REGISTER(stand.relax.allowedDeviation) = 5;   // [mm]
  PARAMETER_ANGLE_REGISTER(stand.relax.allowedRotationDeviation) = 5; // [rad]
  //PARAMETER_REGISTER(stand.relax.timeBonusForCorrection)   = 1000; // [ms]

  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.enable) = true;
  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.deadTime)         = 1000;         // [ms]
  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.currentThreshold) = 0.3;          // [A]
  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.minimalJointStep) = 0.0013962634; // [rad]

  PARAMETER_REGISTER(stand.relax.stiffnessControl.enable) = true;
  //PARAMETER_REGISTER(stand.relax.stiffnessControl.deadTime)     = 100;  // [ms]
  PARAMETER_REGISTER(stand.relax.stiffnessControl.minAngle)     = 0.08; // [°]
  PARAMETER_REGISTER(stand.relax.stiffnessControl.maxAngle)     = 2;    // [°]
  PARAMETER_REGISTER(stand.relax.stiffnessControl.minStiffness) = 0.3;
  PARAMETER_REGISTER(stand.relax.stiffnessControl.maxStiffness) = 1.0;

  // walk parameter:
  // General
  PARAMETER_ANGLE_REGISTER(walk.general.bodyPitchOffset) = 0.2;
  PARAMETER_REGISTER(walk.general.hipOffsetX) = 15;
  PARAMETER_REGISTER(walk.general.stiffness) = 0.7;
  PARAMETER_REGISTER(walk.general.stiffnessArms) = 0.7;
  PARAMETER_REGISTER(walk.general.useArm) = false;
  PARAMETER_REGISTER(walk.general.hipRollSingleSupFactorLeft) = 0.4;
  PARAMETER_REGISTER(walk.general.hipRollSingleSupFactorRight) = 0.4;

  // hip trajectory geometry
  PARAMETER_REGISTER(walk.hip.comHeight) = 260;
  PARAMETER_REGISTER(walk.hip.comHeightOffset) = 0.18;
  PARAMETER_REGISTER(walk.hip.comStepOffsetY) = 0;
  PARAMETER_REGISTER(walk.hip.ZMPOffsetY) = 5;
  PARAMETER_REGISTER(walk.hip.ZMPOffsetYByCharacter) = 0;

  // experimental: new ZMP
  PARAMETER_REGISTER(walk.hip.newZMP_ON) = false;

  PARAMETER_REGISTER(walk.zmp.bezier.transitionScaling) = 0.6;
  PARAMETER_REGISTER(walk.zmp.bezier.inFootScalingY) = 1;
  PARAMETER_REGISTER(walk.zmp.bezier.inFootSpacing)  = 10;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetX) = 20;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetY) = 0;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetXForKicks) = 0;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetYForKicks) = 0;

  PARAMETER_REGISTER(walk.zmp.bezier2.offsetY) = 0;
  PARAMETER_REGISTER(walk.zmp.bezier2.offsetT) = 0;

  // step geometry
  PARAMETER_REGISTER(walk.step.duration) = 300;
  PARAMETER_REGISTER(walk.step.dynamicDuration) = true;
  PARAMETER_REGISTER(walk.step.doubleSupportTime) = 40;
  PARAMETER_REGISTER(walk.step.stepHeight) = 15;
  PARAMETER_REGISTER(walk.step.splineFootTrajectory) = true;

  // kick
  PARAMETER_REGISTER(walk.kick.stepHeight) = 20;
  PARAMETER_REGISTER(walk.kick.ZMPOffsetY) = 0;

  // step limits
  PARAMETER_REGISTER(walk.limits.maxTurnInner) = 10;
  PARAMETER_REGISTER(walk.limits.maxStepTurn) = 30;
  PARAMETER_REGISTER(walk.limits.maxStepLength) = 50;
  PARAMETER_REGISTER(walk.limits.maxStepLengthBack) = 50;
  PARAMETER_REGISTER(walk.limits.maxStepWidth) = 50;
  PARAMETER_REGISTER(walk.limits.maxStepChange) = 0.5;

  // step control
  PARAMETER_REGISTER(walk.limits.maxCtrlTurn) = 30;
  PARAMETER_REGISTER(walk.limits.maxCtrlLength) = 80;
  PARAMETER_REGISTER(walk.limits.maxCtrlWidth) = 50;

  // Stabilization
  //PARAMETER_REGISTER(walk.stabilization.enableFSRProtection) = true;
  //PARAMETER_REGISTER(walk.stabilization.maxWaitLandingCount) = 20;
  //PARAMETER_REGISTER(walk.stabilization.enableWaitLanding) = false;
  //PARAMETER_REGISTER(walk.stabilization.minFSRProtectionCount) = 0;
  //PARAMETER_REGISTER(walk.stabilization.maxUnsupportedCount) = 0;

  PARAMETER_REGISTER(walk.stabilization.emergencyStopError) = 500;

  PARAMETER_REGISTER(walk.stabilization.rotationStabilize) = true;
  PARAMETER_REGISTER(walk.stabilization.rotationStabilizeRC16) = false;
  PARAMETER_REGISTER(walk.stabilization.rotationStabilizeNewIMU) = false;
  PARAMETER_REGISTER(walk.stabilization.rotationP.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationP.y) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationVelocityP.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationVelocityP.y) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationD.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationD.y) = 0;

  PARAMETER_REGISTER(walk.stabilization.stabilizeFeet) = true;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetP.x) = 0.04;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetP.y) = 0.035;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetD.x) = -0.4;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetD.y) = -0.3;

  PARAMETER_REGISTER(walk.stabilization.dynamicStepsize) = true;
  PARAMETER_REGISTER(walk.stabilization.dynamicStepsizeP) = -1;
  PARAMETER_REGISTER(walk.stabilization.dynamicStepsizeD) = 0.5;

  PARAMETER_REGISTER(walk.stabilization.hipOffsetBasedOnStepChange.x) = 0.0;
  PARAMETER_REGISTER(walk.stabilization.hipOffsetBasedOnStepChange.y) = 0.0;

  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLength.x) = 5;
  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLength.y) = 0;
  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLengthForKicks.x) = 5;
  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLengthForKicks.y) = 0;

//  // rotation stabilize parameter
//  PARAMETER_REGISTER(rotationStabilize.k.x) = -0.5;
//  PARAMETER_REGISTER(rotationStabilize.k.y) = -0.2;
//  PARAMETER_REGISTER(rotationStabilize.threshold.x) = 2;
//  PARAMETER_REGISTER(rotationStabilize.threshold.y) = 3;

  // arm parameter
  PARAMETER_REGISTER(arm.inertialModelBasedMovement.shoulderPitchInterialSensorRate) = -10;
  PARAMETER_REGISTER(arm.inertialModelBasedMovement.shoulderRollInterialSensorRate)  = -10;
  PARAMETER_REGISTER(arm.synchronisedWithWalk.shoulderPitchRate) = 0.5;
  PARAMETER_REGISTER(arm.synchronisedWithWalk.shoulderRollRate)  = 0.5;
  PARAMETER_REGISTER(arm.synchronisedWithWalk.elbowRollRate)     = 0.5;
  PARAMETER_REGISTER(arm.maxSpeed) = 60;

  PARAMETER_REGISTER(balanceCoM.kP) = 0;
  PARAMETER_REGISTER(balanceCoM.kI) = 0;
  PARAMETER_REGISTER(balanceCoM.kD) = 0;
  PARAMETER_REGISTER(balanceCoM.threshold) = 10;

  syncWithConfig();
}

IKParameters::~IKParameters()
{
}
<commit_msg>some reasonable default parameter<commit_after>/**
* @file IKParameters.cpp
*
* @author Xu, Yuan
* Implement of parameters for IK motion
*/

#include "IKParameters.h"

IKParameters::IKParameters()
:ParameterList("IKParameters")
{
  PARAMETER_REGISTER(footOffsetY) = 0;

  // stand parameter
  PARAMETER_REGISTER(stand.speed) = 0.04;
  PARAMETER_REGISTER(stand.enableStabilization) = true;
  PARAMETER_REGISTER(stand.enableStabilizationRC16) = false;
  PARAMETER_REGISTER(stand.stiffness) = 0.7;

  PARAMETER_ANGLE_REGISTER(stand.bodyPitchOffset) = 0.2;
  PARAMETER_REGISTER(stand.hipOffsetX) = 15;

  // relax
  PARAMETER_REGISTER(stand.relax.enable) = true;
  PARAMETER_REGISTER(stand.relax.allowedDeviation) = 5;   // [mm]
  PARAMETER_ANGLE_REGISTER(stand.relax.allowedRotationDeviation) = 5; // [rad]
  //PARAMETER_REGISTER(stand.relax.timeBonusForCorrection)   = 1000; // [ms]

  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.enable) = true;
  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.deadTime)         = 1000;         // [ms]
  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.currentThreshold) = 0.3;          // [A]
  PARAMETER_REGISTER(stand.relax.jointOffsetTuning.minimalJointStep) = 0.0013962634; // [rad]

  PARAMETER_REGISTER(stand.relax.stiffnessControl.enable) = true;
  //PARAMETER_REGISTER(stand.relax.stiffnessControl.deadTime)     = 100;  // [ms]
  PARAMETER_REGISTER(stand.relax.stiffnessControl.minAngle)     = 0.08; // [°]
  PARAMETER_REGISTER(stand.relax.stiffnessControl.maxAngle)     = 2;    // [°]
  PARAMETER_REGISTER(stand.relax.stiffnessControl.minStiffness) = 0.3;
  PARAMETER_REGISTER(stand.relax.stiffnessControl.maxStiffness) = 1.0;

  // walk parameter:
  // General
  PARAMETER_ANGLE_REGISTER(walk.general.bodyPitchOffset) = 0.2;
  PARAMETER_REGISTER(walk.general.hipOffsetX) = 15;
  PARAMETER_REGISTER(walk.general.stiffness) = 0.7;
  PARAMETER_REGISTER(walk.general.stiffnessArms) = 0.7;
  PARAMETER_REGISTER(walk.general.useArm) = false;
  PARAMETER_REGISTER(walk.general.hipRollSingleSupFactorLeft) = 0.4;
  PARAMETER_REGISTER(walk.general.hipRollSingleSupFactorRight) = 0.4;

  // hip trajectory geometry
  PARAMETER_REGISTER(walk.hip.comHeight) = 260;
  PARAMETER_REGISTER(walk.hip.comHeightOffset) = 0.18;
  PARAMETER_REGISTER(walk.hip.comStepOffsetY) = 0;
  PARAMETER_REGISTER(walk.hip.ZMPOffsetY) = 5;
  PARAMETER_REGISTER(walk.hip.ZMPOffsetYByCharacter) = 0;

  // experimental: new ZMP
  PARAMETER_REGISTER(walk.hip.newZMP_ON) = false;

  PARAMETER_REGISTER(walk.zmp.bezier.transitionScaling) = 0.6;
  PARAMETER_REGISTER(walk.zmp.bezier.inFootScalingY) = 1;
  PARAMETER_REGISTER(walk.zmp.bezier.inFootSpacing)  = 10;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetX) = 20;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetY) = -5;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetXForKicks) = 0;
  PARAMETER_REGISTER(walk.zmp.bezier.offsetYForKicks) = -20;

  PARAMETER_REGISTER(walk.zmp.bezier2.offsetY) = 0;
  PARAMETER_REGISTER(walk.zmp.bezier2.offsetT) = 0;

  // step geometry
  PARAMETER_REGISTER(walk.step.duration) = 300;
  PARAMETER_REGISTER(walk.step.dynamicDuration) = true;
  PARAMETER_REGISTER(walk.step.doubleSupportTime) = 40;
  PARAMETER_REGISTER(walk.step.stepHeight) = 15;
  PARAMETER_REGISTER(walk.step.splineFootTrajectory) = true;

  // kick
  PARAMETER_REGISTER(walk.kick.stepHeight) = 20;
  PARAMETER_REGISTER(walk.kick.ZMPOffsetY) = 0;

  // step limits
  PARAMETER_REGISTER(walk.limits.maxTurnInner) = 10;
  PARAMETER_REGISTER(walk.limits.maxStepTurn) = 30;
  PARAMETER_REGISTER(walk.limits.maxStepLength) = 50;
  PARAMETER_REGISTER(walk.limits.maxStepLengthBack) = 50;
  PARAMETER_REGISTER(walk.limits.maxStepWidth) = 50;
  PARAMETER_REGISTER(walk.limits.maxStepChange) = 0.5;

  // step control
  PARAMETER_REGISTER(walk.limits.maxCtrlTurn) = 30;
  PARAMETER_REGISTER(walk.limits.maxCtrlLength) = 80;
  PARAMETER_REGISTER(walk.limits.maxCtrlWidth) = 50;

  // Stabilization
  //PARAMETER_REGISTER(walk.stabilization.enableFSRProtection) = true;
  //PARAMETER_REGISTER(walk.stabilization.maxWaitLandingCount) = 20;
  //PARAMETER_REGISTER(walk.stabilization.enableWaitLanding) = false;
  //PARAMETER_REGISTER(walk.stabilization.minFSRProtectionCount) = 0;
  //PARAMETER_REGISTER(walk.stabilization.maxUnsupportedCount) = 0;

  PARAMETER_REGISTER(walk.stabilization.emergencyStopError) = 500;

  PARAMETER_REGISTER(walk.stabilization.rotationStabilize) = true;
  PARAMETER_REGISTER(walk.stabilization.rotationStabilizeRC16) = false;
  PARAMETER_REGISTER(walk.stabilization.rotationStabilizeNewIMU) = false;
  PARAMETER_REGISTER(walk.stabilization.rotationP.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationP.y) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationVelocityP.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationVelocityP.y) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationD.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.rotationD.y) = 0;

  PARAMETER_REGISTER(walk.stabilization.stabilizeFeet) = true;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetP.x) = 0.04;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetP.y) = 0.035;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetD.x) = -0.4;
  PARAMETER_REGISTER(walk.stabilization.stabilizeFeetD.y) = -0.3;

  PARAMETER_REGISTER(walk.stabilization.dynamicStepsize) = true;
  PARAMETER_REGISTER(walk.stabilization.dynamicStepsizeP) = -1;
  PARAMETER_REGISTER(walk.stabilization.dynamicStepsizeD) = 0.5;

  PARAMETER_REGISTER(walk.stabilization.hipOffsetBasedOnStepChange.x) = 0.0;
  PARAMETER_REGISTER(walk.stabilization.hipOffsetBasedOnStepChange.y) = 0.0;

  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLength.x) = 5;
  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLength.y) = 0;
  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLengthForKicks.x) = 0;
  PARAMETER_REGISTER(walk.stabilization.maxHipOffsetBasedOnStepLengthForKicks.y) = 0;

//  // rotation stabilize parameter
//  PARAMETER_REGISTER(rotationStabilize.k.x) = -0.5;
//  PARAMETER_REGISTER(rotationStabilize.k.y) = -0.2;
//  PARAMETER_REGISTER(rotationStabilize.threshold.x) = 2;
//  PARAMETER_REGISTER(rotationStabilize.threshold.y) = 3;

  // arm parameter
  PARAMETER_REGISTER(arm.inertialModelBasedMovement.shoulderPitchInterialSensorRate) = -10;
  PARAMETER_REGISTER(arm.inertialModelBasedMovement.shoulderRollInterialSensorRate)  = -10;
  PARAMETER_REGISTER(arm.synchronisedWithWalk.shoulderPitchRate) = 0.5;
  PARAMETER_REGISTER(arm.synchronisedWithWalk.shoulderRollRate)  = 0.5;
  PARAMETER_REGISTER(arm.synchronisedWithWalk.elbowRollRate)     = 0.5;
  PARAMETER_REGISTER(arm.maxSpeed) = 60;

  PARAMETER_REGISTER(balanceCoM.kP) = 0;
  PARAMETER_REGISTER(balanceCoM.kI) = 0;
  PARAMETER_REGISTER(balanceCoM.kD) = 0;
  PARAMETER_REGISTER(balanceCoM.threshold) = 10;

  syncWithConfig();
}

IKParameters::~IKParameters()
{
}
<|endoftext|>


<commit_before>#include <iostream>
#include <string>
#include "consoleui.h"
#include "scientist.h"
#include <iomanip>

using namespace std;

ConsoleUI::ConsoleUI()
{
    _service.getScientists();  // gets information from file so it's used from the beginning.
}

// Should not contain logic for individual commands, that should be in separate functions!
void ConsoleUI::run()
{

    string command;
    cout << string( 100, '\n' ); // Clears screen

    while(true)
    {

        cout << "Select one of the following options: " << endl;
        cout << "======================================================================" << endl;
        cout << "Add     -   add a programmer/computer scientist" << endl;
        cout << "Remove  -   remove a programmer/computer scientist" << endl;
        cout << "List    -   show a list of all programmer's/computer scientist's" << endl;
        cout << "Search  -   search the list of programmer's/computer scientist's" << endl;
        cout << "Sort    -   sort list by name, gender, age or year of death" << endl;
        cout << "Quit    -   end program" << endl;

        cout << "Select: ";
        cin >> command;
        forceLowerCase(command);

        if(command == "add")
        {
           userMenuAdd();
        }
        else if(command == "remove")
        {
           userMenuRemove();
        }
        else if(command == "list")
        {
           userMenuList();
        }
        else if(command == "search")
        {
           userMenuSearch();
        }
        else if(command == "sort")
        {
            userMenuSort();
        }
        else if(command == "quit")
        {
            cout << string( 100, '\n' ); // Clears screen
            break;
        }
        else
        {
            cout << string( 100, '\n' ); // Clears screen
            cout << "Invalid input" << endl;
            cout << endl;
        }
    }
}
void ConsoleUI::userMenuAdd()
{
    string name;
    string genderInput;
    char gender;
    int birthYear;
    int deathYear = 0;
    int age = 0;
    int a;

    while(true)
    {
        cout << string( 100, '\n' ); // Clears screen

        cout << "Enter the programmer's/computer scientist's name: ";
        cin.ignore();
        getline(cin, name);

        cout << string( 100, '\n' ); // Clears screen
        // Check for gender
        while(true)
        {
            cout << "Enter the programmer's/computer scientist's gender (m/f): ";
            cin >> genderInput;
            forceLowerCase(genderInput);

            if((genderInput == "m") || (genderInput == "f"))
            {
                gender = genderInput[0];
                cin.clear();
                cin.ignore(INT_MAX, '\n');
                break;

            }
            else
            {
                cout << string( 100, '\n' ); // Clears screen
                cout << "Invalid input" << endl;

            }

        }

        cout << string( 100, '\n' ); // Clears screen
        // Check year of birth
        while(true)
        {
            bool inputCheck;
            do
            {
                cout << "Enter the programmer's/computer scientist's year of birth: ";
                cin >> birthYear;
                inputCheck = cin.fail();
                if(inputCheck)
                {
                    cout << string( 100, '\n' ); // Clears screen
                    cout << "Invalid input" << endl;
                }
                cin.clear();
                cin.ignore(INT_MAX, '\n');

            }while(inputCheck);

            if(birthYear < 2016) // Just in case we find a programmer of the univers
            {
                break;
            }
            else
            {
                cout << string( 100, '\n' ); // Clears screen
                cout << "Invalid input" << endl;
            }
        }

        cout << string( 100, '\n' ); // Clears screen
        // Check when year of death (if dead)
        while(true)
        {
            bool inputCheck;
            do
            {

                cout << "Enter the programmer's/computer scientist's year of death (type 0 if not applicable): ";
                cin >> deathYear;
                inputCheck = cin.fail();
                if(inputCheck)
                {
                    cout << string( 100, '\n' ); // Clears screen
                    cout << "Invalid input" << endl;
                }
                cin.clear();
                cin.ignore(INT_MAX, '\n');

            }while(inputCheck);

            if (deathYear == 0)
            {
                break;
            }
            else if(deathYear >= birthYear)
            {
                break;
            }
            else
            {
                cout << string( 100, '\n' ); // Clears screen
                cout << "Invalid input" << endl;
            }
        }

        // Check if input is correct
        cout << string( 100, '\n' ); // Clears screen

        cout << "Name: " << name << endl << "Gender: " << gender << endl << "Born: " << birthYear << endl;

        if(deathYear != 0)
        {
            cout << "Died: " << deathYear << endl;
        }
        else
        {
            cout << endl;
        }

        a = userCheckInput();

        if (a == 0)
        {
            // false sama nafn
            if(_service.addScientist(name, gender, birthYear, deathYear, age))
            {
                break;
            }
            else
            {
                int userInput;
                cout << string( 100, '\n' ); // Clears screen

                cout << "This name is allready taken, replace existing name(1), start over(2)" << endl;
                cout << "Select: ";
                cin >> userInput;
                if(userInput == 1)
                {
                    _service.removeScientist(name);
                    _service.addScientist(name, gender, birthYear, deathYear, age);
                    break;
                }

            }
        }
        else if (a == 2)
        {
            break;
        }

    }
    cout << endl;

}
void ConsoleUI::userMenuList()
{
    vector<Scientist> scientist = _service.getScientists();
    userMenuPrint(scientist);
}
void ConsoleUI::userMenuSearch()
{
    string command;

    cout << string( 100, '\n' ); // Clears screen
    cout << "Select a search option: " << endl;
    cout << "======================================================================" << endl;
    cout << "Name    -   Search by name" << endl;
    cout << "Gender  -   Search by gender" << endl;
    cout << "Age     -   Search by age" << endl;
    cout << "Birth   -   search by year of birth" << endl;
    cout << "Death   -   search by year of death" << endl;
    cout << "Select: ";
    cin >> command;
    cin.clear();
    cin.ignore(INT_MAX, '\n');
    cout << endl;

    forceLowerCase(command);

    if(command == "name")
    {
        string userInputName;
        cout << string( 100, '\n' ); // Clears screen

        cout << "Search by name: ";
        getline(cin, userInputName);

        vector<Scientist> scientist = _service.findScientistByName(userInputName);
        userMenuPrint(scientist);

    }
    else if(command == "gender") // findScientistByGender
    {
        char userInputGender;
        cout << string( 100, '\n' ); // Clears screen

        cout << "Search by gender: ";
        cin >> userInputGender;

        vector<Scientist> scientist = _service.findScientistByGender(userInputGender);
        userMenuPrint(scientist);
    }
    else if(command == "age") // findScientistByGender
    {
        int inputCheck;

        cout << "To search by age(1), to search by range of age(2)" << endl;
        cin >> inputCheck;
        if(inputCheck == 1)
        {
            int userInputAge;
            cout << "Search by age: ";
            cout << "Select: ";
            cin >> userInputAge;

            vector<Scientist> scientist = _service.findScientistByAge(userInputAge);
            userMenuPrint(scientist);
        }
        else if(inputCheck == 2)
        {
            int userInputAgeFirst;
            int userInputAgeLast;
            cout << "Search from age: ";
            cin >> userInputAgeFirst;
            cout << endl;
            cout << "to age: ";
            cin >> userInputAgeLast;
            cout << endl;

            vector<Scientist> scientist = _service.findScientistByAgeRange(userInputAgeFirst, userInputAgeLast);
            userMenuPrint(scientist);
        }
        else
        {
            cout << string( 100, '\n' ); // Clears screen
            cout << "Wrong Input" << endl;
        }

      //  cout << string( 100, '\n' ); // Clears screen

    }
    else if(command == "birth")
    {
        int inputCheck;

        cout << "To search by year of birth(1), to search by range of year of birth(2)" << endl;
        cin >> inputCheck;

        if(inputCheck == 1)
        {
            int userInputBirth;

            cout << "Search by year of birth: ";
            cin >> userInputBirth;

            vector<Scientist> scientist = _service.findScientistByBirth(userInputBirth);
            userMenuPrint(scientist);
        }
        else if(inputCheck == 2)
        {
            int userInputBirthFirst;
            int userInputBirthLast;

            cout << "Search from year of birth: ";
            cin >> userInputBirthFirst;
            cout << endl;
            cout << "to year of birth: ";
            cin >> userInputBirthLast;
            cout << endl;

            vector<Scientist> scientist = _service.findScientistByBirthRange(userInputBirthFirst, userInputBirthLast);
            userMenuPrint(scientist);
        }
        else
        {
            cout << string( 100, '\n' ); // Clears screen
            cout << "Wrong Input" << endl;
        }

    }
    else if(command == "death")
    {
        int inputCheck;

        cout << "To search by year of death(1), to search by range of year of death(2)" << endl;
        cin >> inputCheck;

        if(inputCheck == 1)
        {
            int userInputDeath;

            cout << "Search by year of death (0 for still alive): ";
            cin >> userInputDeath;

            vector<Scientist> scientist = _service.findScientistByDeath(userInputDeath);
            userMenuPrint(scientist);
        }
        else if(inputCheck == 2)
        {
            int userInputDeathFirst;
            int userInputDeathLast;

            cout << "Search from year of death (0 for still alive): ";
            cin >> userInputDeathFirst;
            cout << endl;
            cout << "to year of death (0 for still alive): ";
            cin >> userInputDeathLast;
            cout << endl;

            vector<Scientist> scientist = _service.findScientistByDeathRange(userInputDeathFirst, userInputDeathLast);
            userMenuPrint(scientist);
        }
        else
        {
            cout << string( 100, '\n' ); // Clears screen
            cout << "Wrong Input" << endl;
        }

    }
    cout << endl;

}
void ConsoleUI::userMenuSort()
{
    bool inputCheck = true;
    int userInput;

    do
    {
        cout << string( 100, '\n' ); // Clears screen

        cout << "Sort list by Name A-Z(1), Name Z-A(2), Gender(3), Year of Birth(4), Year of Death(5) or Age (6)" << endl;
        cout << "Select: ";
        cin >> userInput;

        if(userInput > 0 && userInput < 7) // check if input is int and if it ranges from 1 to 6
        {
            inputCheck = false;
        }
        else if(cin.fail())
        {
            cout << "Invalid input" << endl;
        }
        else
        {
            cout << "Invalid input" << endl;
        }
        cin.clear();
        cin.ignore(INT_MAX, '\n');
    }while(inputCheck);

     _service.scientistSort(userInput);
     userMenuList();
}
void ConsoleUI::userMenuPrint(vector<Scientist>scientist)
{
    cout << string( 100, '\n' );
    cout << left << setw(30) << "Scientist name:"
         << setw(10) << right << "gender:"
         << setw(10) << "born:"
         << setw(10) << "died:"
         << setw(10) << "age:" << endl;
    cout << "======================================================================" << endl;
    for (size_t i = 0; i< scientist.size(); ++i)
    {
        cout << left << setw(30) << scientist[i].getName()
             << setw(10) << right << scientist[i].getGender()
             << setw(10) << scientist[i].getBirth();

             if(scientist[i].getDeath() == 0)
             {
                 cout << setw(10) << "-";
             }
             else
             {
                 cout << setw(10) << scientist[i].getDeath();
             }
             cout << setw(10) << scientist[i].getAge() << endl;

    }
    cout << "======================================================================" << endl;
    cout << "To return to menu input the letter m" << string( 2, '\n' );

   string userInput = " ";
   while (userInput != "m")
   {
       cin >> userInput;
   }
}
int  ConsoleUI::userCheckInput()
{
    // Check if all data is correct
    while(true)
    {
        char answear;
        cout << "Is this data correct? (input y/n) or press q to quit" << endl;
        cout << "Select: ";
        cin >> answear;

        if(answear == 'y')
        {
            return 0;
        }
        else if (answear == 'n')
        {
            return 1;
        }
        else if (answear == 'q')
        {
            return 2;
        }
        else
        {
            cout << "Invalid input!";
        }

    }
}
void ConsoleUI::userMenuRemove()
{
    string userInputName;
    cout << string( 100, '\n' ); // Clears screen

    cout << "Remove a programmer/computer scientist: ";
    cin.ignore();
    getline(cin, userInputName);

    _service.removeScientist(userInputName);
    cout << string( 100, '\n' ); // Clears screen
    userMenuList();
}
void ConsoleUI::forceLowerCase(string &command)
{
    for(unsigned int i = 0; i < command.length(); i++) // to make all lowercase, taken from c++ site
    {
        command[i] = tolower(command[i]);
    }
}
<commit_msg>remove all, ui version<commit_after>#include <iostream>
#include <string>
#include "consoleui.h"
#include "scientist.h"
#include <iomanip>

using namespace std;

ConsoleUI::ConsoleUI()
{
    _service.getScientists();  // gets information from file so it's used from the beginning.
}
void ConsoleUI::run()                                       // The main function
{
    /*
     * This is the main function body, that is run at the start of the program every time
     * and gives the user the option to run a few functions like, userMenuAdd(), userMenuList()
     * userMenuSearch(), userMenuSort(), userMenuPrint(). User can also end the program from here.
     */

    string command;
    cout << string( 100, '\n' );

    while(true)
    {
        cout << "Select one of the following options: " << endl;
        cout << "======================================================================" << endl;
        cout << "Add     -   add a programmer/computer scientist" << endl;
        cout << "Remove  -   remove a programmer/computer scientist" << endl;
        cout << "List    -   show a list of all programmer's/computer scientist's" << endl;
        cout << "Search  -   search the list of programmer's/computer scientist's" << endl;
        cout << "Sort    -   sort list by name, gender, age or year of death" << endl;
        cout << "Quit    -   end program" << endl;

        cout << "Select: ";
        cin >> command;
        forceLowerCase(command);

        if(command == "add")
        {
           userMenuAdd();
        }
        else if(command == "remove")
        {
           userMenuRemove();
        }
        else if(command == "list")
        {
           userMenuList();
        }
        else if(command == "search")
        {
           userMenuSearch();
        }
        else if(command == "sort")
        {
            userMenuSort();
        }
        else if(command == "quit")
        {
            cout << string( 100, '\n' );
            break;
        }
        else
        {
            cout << string( 100, '\n' );
            cout << "Invalid input" << endl;
            cout << endl;
        }
    }
}
void ConsoleUI::userMenuAdd()                               // Adds a new programmer
{
    string name;
    string genderInput;
    char gender;
    int birthYear;
    int deathYear = 0;
    int age = 0;
    int a;

    while(true)
    {
        cout << string( 100, '\n' );

        cout << "Enter the programmer's/computer scientist's name: ";
        cin.ignore();
        getline(cin, name);

        cout << string( 100, '\n' );
        // Check for gender
        while(true)
        {
            cout << "Enter the programmer's/computer scientist's gender (m/f): ";
            cin >> genderInput;
            forceLowerCase(genderInput);

            if((genderInput == "m") || (genderInput == "f"))
            {
                gender = genderInput[0];
                cin.clear();
                cin.ignore(INT_MAX, '\n');
                break;

            }
            else
            {
                cout << string( 100, '\n' );
                cout << "Invalid input" << endl;

            }

        }

        cout << string( 100, '\n' );
        // Check year of birth
        while(true)
        {
            bool inputCheck;
            do
            {
                cout << "Enter the programmer's/computer scientist's year of birth: ";
                cin >> birthYear;
                inputCheck = cin.fail();
                if(inputCheck)
                {
                    cout << string( 100, '\n' );
                    cout << "Invalid input" << endl;
                }
                cin.clear();
                cin.ignore(INT_MAX, '\n');

            }while(inputCheck);

            if(birthYear < 2016) // Just in case we find a programmer of the univers
            {
                break;
            }
            else
            {
                cout << string( 100, '\n' );
                cout << "Invalid input" << endl;
            }
        }

        cout << string( 100, '\n' );
        // Check when year of death (if dead)
        while(true)
        {
            bool inputCheck;
            do
            {

                cout << "Enter the programmer's/computer scientist's year of death (type 0 if not applicable): ";
                cin >> deathYear;
                inputCheck = cin.fail();
                if(inputCheck)
                {
                    cout << string( 100, '\n' );
                    cout << "Invalid input" << endl;
                }
                cin.clear();
                cin.ignore(INT_MAX, '\n');

            }while(inputCheck);

            if (deathYear == 0)
            {
                break;
            }
            else if(deathYear >= birthYear)
            {
                break;
            }
            else
            {
                cout << string( 100, '\n' );
                cout << "Invalid input" << endl;
            }
        }

        // Check if input is correct
        cout << string( 100, '\n' );

        cout << "Name: " << name << endl << "Gender: " << gender << endl << "Born: " << birthYear << endl;

        if(deathYear != 0)
        {
            cout << "Died: " << deathYear << endl;
        }
        else
        {
            cout << endl;
        }

        a = userCheckInput();

        if (a == 0)
        {
            // false sama nafn
            if(_service.addScientist(name, gender, birthYear, deathYear, age))
            {
                break;
            }
            else
            {
                int userInput;
                cout << string( 100, '\n' );

                cout << "This name is allready taken, replace existing name(1), start over(2)" << endl;
                cout << "Select: ";
                cin >> userInput;
                if(userInput == 1)
                {
                    _service.removeScientist(name);
                    _service.addScientist(name, gender, birthYear, deathYear, age);
                    break;
                }

            }
        }
        else if (a == 2)
        {
            break;
        }

    }
    cout << endl;

}
void ConsoleUI::userMenuList()                              // List of commands
{
    vector<Scientist> scientist = _service.getScientists();
    userMenuPrint(scientist);
}
void ConsoleUI::userMenuSearch()                            // Search list
{
    string command;

    cout << string( 100, '\n' );
    cout << "Select a search option: " << endl;
    cout << "===================================" << endl;
    cout << "Name    -   Search by name" << endl;
    cout << "Gender  -   Search by gender" << endl;
    cout << "Age     -   Search by age" << endl;
    cout << "Birth   -   search by year of birth" << endl;
    cout << "Death   -   search by year of death" << endl;
    cout << "Select: ";
    cin >> command;
    cin.clear();
    cin.ignore(INT_MAX, '\n');
    cout << endl;

    forceLowerCase(command);

    if(command == "name") // Find scientist by name
    {
        string userInputName;
        cout << string( 100, '\n' );

        cout << "Search by name: ";
        getline(cin, userInputName);

        vector<Scientist> scientist = _service.findScientistByName(userInputName);
        userMenuPrint(scientist);

    }
    else if(command == "gender") // Find scientist by gender
    {
        char userInputGender;
        cout << string( 100, '\n' );

        cout << "Search by gender: ";
        cin >> userInputGender;

        vector<Scientist> scientist = _service.findScientistByGender(userInputGender);
        userMenuPrint(scientist);
    }
    else if(command == "age") // Find scientist by age
    {
        int inputCheck;

        cout << "To search by age(1), to search by range of age(2)" << endl;
        cin >> inputCheck;
        if(inputCheck == 1)
        {
            int userInputAge;
            cout << "Search by age: ";
            cout << "Select: ";
            cin >> userInputAge;

            vector<Scientist> scientist = _service.findScientistByAge(userInputAge);
            userMenuPrint(scientist);
        }
        else if(inputCheck == 2)
        {
            int userInputAgeFirst;
            int userInputAgeLast;
            cout << "Search from age: ";
            cin >> userInputAgeFirst;
            cout << endl;
            cout << "to age: ";
            cin >> userInputAgeLast;
            cout << endl;

            vector<Scientist> scientist = _service.findScientistByAgeRange(userInputAgeFirst, userInputAgeLast);
            userMenuPrint(scientist);
        }
        else
        {
            cout << string( 100, '\n' );
            cout << "Wrong Input" << endl;
        }
    }
    else if(command == "birth")
    {
        int inputCheck;

        cout << "To search by year of birth(1), to search by range of year of birth(2)" << endl;
        cin >> inputCheck;

        if(inputCheck == 1)
        {
            int userInputBirth;

            cout << "Search by year of birth: ";
            cin >> userInputBirth;

            vector<Scientist> scientist = _service.findScientistByBirth(userInputBirth);
            userMenuPrint(scientist);
        }
        else if(inputCheck == 2)
        {
            int userInputBirthFirst;
            int userInputBirthLast;

            cout << "Search from year of birth: ";
            cin >> userInputBirthFirst;
            cout << endl;
            cout << "to year of birth: ";
            cin >> userInputBirthLast;
            cout << endl;

            vector<Scientist> scientist = _service.findScientistByBirthRange(userInputBirthFirst, userInputBirthLast);
            userMenuPrint(scientist);
        }
        else
        {
            cout << string( 100, '\n' );
            cout << "Wrong Input" << endl;
        }

    }
    else if(command == "death")
    {
        int inputCheck;

        cout << "To search by year of death(1), to search by range of year of death(2)" << endl;
        cin >> inputCheck;

        if(inputCheck == 1)
        {
            int userInputDeath;

            cout << "Search by year of death (0 for still alive): ";
            cin >> userInputDeath;

            vector<Scientist> scientist = _service.findScientistByDeath(userInputDeath);
            userMenuPrint(scientist);
        }
        else if(inputCheck == 2)
        {
            int userInputDeathFirst;
            int userInputDeathLast;

            cout << "Search from year of death (0 for still alive): ";
            cin >> userInputDeathFirst;
            cout << endl;
            cout << "to year of death (0 for still alive): ";
            cin >> userInputDeathLast;
            cout << endl;

            vector<Scientist> scientist = _service.findScientistByDeathRange(userInputDeathFirst, userInputDeathLast);
            userMenuPrint(scientist);
        }
        else
        {
            cout << string( 100, '\n' );
            cout << "Wrong Input" << endl;
        }

    }
    cout << endl;

}
void ConsoleUI::userMenuSort()                              // Sort list
{
    bool inputCheck = true;
    int userInput;

    do
    {
        cout << string( 100, '\n' );

        cout << "Sort list by Name A-Z(1), Name Z-A(2), Gender(3), Year of Birth(4), Year of Death(5) or Age (6)" << endl;
        cout << "Select: ";
        cin >> userInput;

        if(userInput > 0 && userInput < 7) // check if input is int and if it ranges from 1 to 6
        {
            inputCheck = false;
        }
        else if(cin.fail())
        {
            cout << "Invalid input" << endl;
        }
        else
        {
            cout << "Invalid input" << endl;
        }
        cin.clear();
        cin.ignore(INT_MAX, '\n');
    }while(inputCheck);

     _service.scientistSort(userInput);
     userMenuList();
}
void ConsoleUI::userMenuPrint(vector<Scientist>scientist)   // Print list
{
    cout << string( 100, '\n' );
    cout << left << setw(30) << "Scientist name:"
         << setw(10) << right << "gender:"
         << setw(10) << "born:"
         << setw(10) << "died:"
         << setw(10) << "age:" << endl;
    cout << "======================================================================" << endl;
    for (size_t i = 0; i< scientist.size(); ++i)
    {
        cout << left << setw(30) << scientist[i].getName()
             << setw(10) << right << scientist[i].getGender()
             << setw(10) << scientist[i].getBirth();

             if(scientist[i].getDeath() == 0)
             {
                 cout << setw(10) << "-";
             }
             else
             {
                 cout << setw(10) << scientist[i].getDeath();
             }
             cout << setw(10) << scientist[i].getAge() << endl;

    }
    cout << "======================================================================" << endl;
    cout << "To return to menu input the letter m" << string( 2, '\n' );

   string userInput = " ";
   while (userInput != "m")
   {
       cin >> userInput;
   }
}
int  ConsoleUI::userCheckInput()                            // Check input from userMenuAdd
{
    // Check if all data is correct
    while(true)
    {
        char answear;
        cout << "Is this data correct? (input y/n) or press q to quit" << endl;
        cout << "Select: ";
        cin >> answear;

        if(answear == 'y')
        {
            return 0;
        }
        else if (answear == 'n')
        {
            return 1;
        }
        else if (answear == 'q')
        {
            return 2;
        }
        else
        {
            cout << "Invalid input!";
        }

    }
}
void ConsoleUI::userMenuRemove()                            // Removes a programmer
{
    int command;

    cin >> command;
    cin.clear();
    cin.ignore(INT_MAX, '\n');
    cout << endl;

    if(command == 1) // Remove 1 programmer
    {
        string userInputName;
        cout << string( 100, '\n' );

        cout << "Remove a programmer/computer scientist: ";
        cin.ignore();
        getline(cin, userInputName);

        _service.removeScientist(userInputName);
        cout << string( 100, '\n' );
        userMenuList();
    }
    else if(command == 2) // Remove all programmers
    {
        string userInputName;
        cout << string( 100, '\n' );

        cout << "Remove a programmer/computer scientist: ";
        cin.ignore();
        getline(cin, userInputName);

        _service.removeAllScientist(userInputName);
        cout << string( 100, '\n' );
        userMenuList();
    }
    else
    {
        cout << string( 100, '\n' );
        cout << "Wrong Input" << endl;
    }
}
void ConsoleUI::forceLowerCase(string &command)             // Force input to lower case
{
    for(unsigned int i = 0; i < command.length(); i++) // to make all lowercase, taken from c++ site
    {
        command[i] = tolower(command[i]);
    }
}
<|endoftext|>


<commit_before>#include "linedetector.h"
#include <opencv2/video/video.hpp>
#include <opencv2/opencv.hpp>
#include <sensor_msgs/image_encodings.h>
#include <pcl_ros/point_cloud.h>
#include <queue>
#include <chrono>
#include <ctime>

using namespace std;
using namespace cv;
using namespace pcl;

cv_bridge::CvImagePtr cv_ptr;
typedef pcl::PointCloud<pcl::PointXYZ> PCLCloud;

constexpr double radians(double degrees)
{
    return degrees / 180.0 * M_PI;
}

void LineDetector::img_callback(const sensor_msgs::ImageConstPtr& msg) {
    // chrono::time_point<chrono::system_clock> start, end; // instantiate time to use
    // start = chrono::system_clock::now();

    cv_ptr = cv_bridge::toCvCopy(msg, "");
    src_img = cv_ptr->image;
    dst_img = Mat::zeros(src_img.size(), src_img.type());
    fin_img = Mat::zeros(src_img.size(), src_img.type());

    // What separates lines from other objects?
    // 1. Lines are whiter than their surroundings
    // 2. Lines are on the ground
    // 3. A section of a line has two other line sections adjascent to it
    // 4. We know roughly how wide a line is going to be

    DetectLines(lineThickness);

    EnforceLength(working, lineLengthThreshold);

    cvtColor(working, working, CV_GRAY2BGR);
    resize(working, fin_img, Size(src_img.cols, src_img.rows), 0, 0, INTER_LANCZOS4);

    cloud = toPointCloud(fin_img);
    _line_cloud.publish(cloud);

    cv_ptr->image = fin_img;
    _filt_img.publish(cv_ptr->toImageMsg());

    // end = chrono::system_clock::now();
    // chrono::duration<double> elapsedTime = end - start;

    // cerr<<elapsedTime.count();
}

typedef struct Node {
    shared_ptr<struct Node> prev;
    Point point;
    int length;

    Node(shared_ptr<struct Node> pr, Point po, int l)
    {
        prev = pr;
        point = po;
        length = l;
    }
} Node;

LineDetector::LineDetector(ros::NodeHandle &handle)
      : _it(handle)
      , tf_listener(handle)
{
     cout<<"Running"<<endl;
    _src_img = _it.subscribe("/stereo/left/image_raw", 1, &LineDetector::img_callback, this);
	  _filt_img = _it.advertise("/filt_img", 1);
    _line_cloud = handle.advertise<PCLCloud>("/line_cloud", 100);
    initLineDetection();
}

PointCloud<PointXYZ>::Ptr LineDetector::toPointCloud(Mat src){
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    tf::StampedTransform transform;
    tf_listener.lookupTransform("/base_footprint", "/camera_left", ros::Time(0), transform);
    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
    auto origin_z = transform.getOrigin().getZ();
    auto origin_y = transform.getOrigin().getY();
    auto HFOV = radians(66.0);
    auto VFOV = radians(47.6);
    pitch = -roll; // Because conventions are different and I'm in the middle of comp, and give me a break.
    for(int r = src.rows/2; r < src.rows; r++)
    {
        uchar *row = src.ptr<uchar>(r);
        for(int c = 0; c < src.cols; c++)
        {
            if(row[c] > 0)
            {
                auto pitch_offset = ((float)(r-src.rows/2) / src.rows) * VFOV;
                auto y = origin_z /tan(pitch + pitch_offset) + origin_y;

                auto theta = ((float)(c-src.cols/2) / src.cols) * HFOV;
                auto x = y * tan(theta);

                cloud->points.push_back(PointXYZ(x, y, 0));
            }
        }
    }
    cloud->header.frame_id = "base_footprint";
    return cloud;
}

void LineDetector::initLineDetection() {
    cerr << "DetectLines::Initing" << endl;
    lineThickness = 20;
    lineLengthThreshold = 30;

    float karray[3][9][9] = {
            {
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {1,     1,     1,     1,     1,     1,     1,     1,     1},
                    {1,     1,     1,     1,     1,     1,     1,     1,     1},
                    {1,     1,     1,     1,     1,     1,     1,     1,     1},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0}
            },
            {
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,     0,     1},
                    {-1,   -1,    -1,    -1,    -1,     0,     1,     1,     1},
                    {-1,   -1,    -1,     0,     1,     1,     1,     1,     1},
                    {-1,    0,     1,     1,     1,     1,     1,    .5,     0},
                    {1,     1,     1,     1,     1,    .5,     0,     0,     0},
                    {1,     1,     1,    .5,     0,     0,     0,     0,     0},
                    {1,    .5,     0,     0,     0,     0,     0,     0,     0},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0}
            },
            {
                    {-.89,-.89, -.89,  -.89,  -.89,  -.89,  -.89,     1,     1},
                    {-.89,-.89, -.89,  -.89,  -.89,  -.89,     1,     1,     1},
                    {-.89,-.89, -.89,  -.89,  -.89,     1,     1,     1,     0},
                    {-.89,-.89, -.89,  -.89,     1,     1,     1,     0,     0},
                    {-.89,-.89, -.89,     1,     1,     1,     0,     0,     0},
                    {-.89,-.89,    1,     1,     1,     0,     0,     0,     0},
                    {-.89,  1,     1,     1,     0,     0,     0,     0,     0},
                    {1,     1,     1,     0,     0,     0,     0,     0,     0},
                    {1,     1,     0,     0,     0,     0,     0,     0,     0}
            }
    };

    Mat kernel1(9, 9, CV_32FC1, karray[0]);
    kernel1 /= 27;
    Mat kernel2(9, 9, CV_32FC1, karray[1]);
    kernel2 /= 25;
    Mat kernel3(9, 9, CV_32FC1, karray[2]);
    kernel3 /= 25;

    Mat kernel4 = kernel2.t();
    Mat kernel5 = kernel1.t();

    Mat kernel6;
    Mat kernel7;
    Mat kernel8;

    flip(kernel4, kernel6, 0);
    flip(kernel3, kernel7, 0);
    flip(kernel2, kernel8, 0);

    kernels[0] = kernel1.clone();
    kernels[1] = kernel2.clone();
    kernels[2] = kernel3.clone();
    kernels[3] = kernel4.clone();
    kernels[4] = kernel5.clone();
    kernels[5] = kernel6.clone();
    kernels[6] = kernel7.clone();
    kernels[7] = kernel8.clone();

    for (int i = 0; i < KERNAL_COUNT; i++) {
        Mat kernelComplement;
        flip(kernels[i], kernelComplement, -1);
        kernelComplements[i] = kernelComplement.clone();
    }
}

void LineDetector::DetectLines(int lineThickness) {

    // Resize the image such that the lines are approximately 3 pixels wide
    //cerr << "DetectLines::Reducing Image" << endl;
    lineThickness = max(1, lineThickness); // 0 thickness doesn't make sense
    resize(src_img, working, Size(3*src_img.cols/lineThickness, 3*src_img.rows/lineThickness), 0, 0, INTER_LANCZOS4);

    // Convert the image into HSV space to make processing white lines easier
    //cerr << "DetectLines::Converting to HSV" << endl;
    cvtColor(working, working, CV_BGR2HSV);

    // Calculate each pixel's "whiteness" defined as value*(255-saturation);
    //cerr << "DetectLines::Filtering Whiteness" << endl;
    WhitenessFilter(working, working);

    // Pass directional kernels over image
    //cerr << "DetectLines::Filtering kernels" << endl;
    for(size_t i = 0; i < KERNAL_COUNT; i++)
        filter2D(working, kernelResults[i], -1, kernels[i]);

    // Pass directional kernel complements over image (same edge, rotated 180 degrees, 3 pixels between edge and complement)
    //cerr << "DetectLines::Filtering complements" << endl;
    for(size_t i = 0; i < KERNAL_COUNT; i++)
        filter2D(working, complementResults[i], -1, kernelComplements[i]);

    // Multiply the results of kernel filter by its complement
    //cerr << "DetectLines::Multiplying Results" << endl;
    MultiplyByComplements(kernelResults, complementResults, kernelResults);

    working = Mat::zeros(kernelResults[0].size(), kernelResults[0].type());
    //cerr << "DetectLines::Thresholding Results" << endl;
    for(int i = 0; i < KERNAL_COUNT; i++) {
        // threshold(kernelResults[i], kernelResults[i], ((float)lineContinue*lineContinue)/255, 255, CV_THRESH_BINARY);
        adaptiveThreshold(kernelResults[i], kernelResults[i], 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 9, -1.85);
        bitwise_or(working, kernelResults[i], working);
    }
}

void LineDetector::WhitenessFilter(Mat& hsv_image, Mat& fin_img) {
    Mat result = 255 * Mat::ones(hsv_image.size(), CV_16UC1);
    Mat tmp;
    hsv_image.convertTo(tmp, CV_16UC3, 1.0);
    Mat channel[3];
    split(tmp, channel);
    result = result - channel[1];
    result = result.mul(channel[2]);
    result.convertTo(fin_img, CV_8UC1, 1.0/255);
}

void LineDetector::MultiplyByComplements(Mat* images, Mat* complements, Mat* results) {
    for(size_t i = 0; i < KERNAL_COUNT; i++) {
        Mat image;
        Mat complement;
        Mat result = Mat::zeros(images[0].size(), CV_16UC1);
        images[i].convertTo(image, CV_16UC1, 1.0);
        complements[i].convertTo(complement, CV_16UC1, 1.0);
        result = image.mul(complement);
        result.convertTo(results[i], CV_8UC1, 1.0/255); // TODO figure this const out
    }
}

void LineDetector::EnforceLength(Mat& img, int length) {
    // Thresholding the line length using the area of the contours
    vector<vector<Point>> contours;
    vector<vector<Point>> contoursThreshold;
    vector<Vec4i> hierarchy;

    Mat copy = img.clone();
    findContours(copy, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

    for (unsigned int i = 0; i < contours.size(); ++i) {
        if ((int) contours[i].size() < length) {
            contoursThreshold.push_back(contours[i]);
        }
    }

    Scalar color(0, 0, 0);
    drawContours(img, contoursThreshold, -1, color, 3);
}
<commit_msg>Updated line detector subscribe topic<commit_after>#include "linedetector.h"
#include <opencv2/video/video.hpp>
#include <opencv2/opencv.hpp>
#include <sensor_msgs/image_encodings.h>
#include <pcl_ros/point_cloud.h>
#include <queue>
#include <chrono>
#include <ctime>

using namespace std;
using namespace cv;
using namespace pcl;

cv_bridge::CvImagePtr cv_ptr;
typedef pcl::PointCloud<pcl::PointXYZ> PCLCloud;

constexpr double radians(double degrees)
{
    return degrees / 180.0 * M_PI;
}

void LineDetector::img_callback(const sensor_msgs::ImageConstPtr& msg) {
    // chrono::time_point<chrono::system_clock> start, end; // instantiate time to use
    // start = chrono::system_clock::now();

    cv_ptr = cv_bridge::toCvCopy(msg, "");
    src_img = cv_ptr->image;
    dst_img = Mat::zeros(src_img.size(), src_img.type());
    fin_img = Mat::zeros(src_img.size(), src_img.type());

    // What separates lines from other objects?
    // 1. Lines are whiter than their surroundings
    // 2. Lines are on the ground
    // 3. A section of a line has two other line sections adjascent to it
    // 4. We know roughly how wide a line is going to be

    DetectLines(lineThickness);

    EnforceLength(working, lineLengthThreshold);

    cvtColor(working, working, CV_GRAY2BGR);
    resize(working, fin_img, Size(src_img.cols, src_img.rows), 0, 0, INTER_LANCZOS4);

    cloud = toPointCloud(fin_img);
    _line_cloud.publish(cloud);

    cv_ptr->image = fin_img;
    _filt_img.publish(cv_ptr->toImageMsg());

    // end = chrono::system_clock::now();
    // chrono::duration<double> elapsedTime = end - start;

    // cerr<<elapsedTime.count();
}

typedef struct Node {
    shared_ptr<struct Node> prev;
    Point point;
    int length;

    Node(shared_ptr<struct Node> pr, Point po, int l)
    {
        prev = pr;
        point = po;
        length = l;
    }
} Node;

LineDetector::LineDetector(ros::NodeHandle &handle)
      : _it(handle)
      , tf_listener(handle)
{
     cout<<"Running"<<endl;
    _src_img = _it.subscribe("/left/image_rect_color", 1, &LineDetector::img_callback, this);
	  _filt_img = _it.advertise("/filt_img", 1);
    _line_cloud = handle.advertise<PCLCloud>("/line_cloud", 100);
    initLineDetection();
}

PointCloud<PointXYZ>::Ptr LineDetector::toPointCloud(Mat src){
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    tf::StampedTransform transform;
    tf_listener.lookupTransform("/base_footprint", "/camera_left", ros::Time(0), transform);
    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getRPY(roll, pitch, yaw);
    auto origin_z = transform.getOrigin().getZ();
    auto origin_y = transform.getOrigin().getY();
    auto HFOV = radians(66.0);
    auto VFOV = radians(47.6);
    pitch = -roll; // Because conventions are different and I'm in the middle of comp, and give me a break.
    for(int r = src.rows/2; r < src.rows; r++)
    {
        uchar *row = src.ptr<uchar>(r);
        for(int c = 0; c < src.cols; c++)
        {
            if(row[c] > 0)
            {
                auto pitch_offset = ((float)(r-src.rows/2) / src.rows) * VFOV;
                auto y = origin_z /tan(pitch + pitch_offset) + origin_y;

                auto theta = ((float)(c-src.cols/2) / src.cols) * HFOV;
                auto x = y * tan(theta);

                cloud->points.push_back(PointXYZ(x, y, 0));
            }
        }
    }
    cloud->header.frame_id = "base_footprint";
    return cloud;
}

void LineDetector::initLineDetection() {
    cerr << "DetectLines::Initing" << endl;
    lineThickness = 20;
    lineLengthThreshold = 30;

    float karray[3][9][9] = {
            {
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {1,     1,     1,     1,     1,     1,     1,     1,     1},
                    {1,     1,     1,     1,     1,     1,     1,     1,     1},
                    {1,     1,     1,     1,     1,     1,     1,     1,     1},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0}
            },
            {
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,    -1,    -1},
                    {-1,   -1,    -1,    -1,    -1,    -1,    -1,     0,     1},
                    {-1,   -1,    -1,    -1,    -1,     0,     1,     1,     1},
                    {-1,   -1,    -1,     0,     1,     1,     1,     1,     1},
                    {-1,    0,     1,     1,     1,     1,     1,    .5,     0},
                    {1,     1,     1,     1,     1,    .5,     0,     0,     0},
                    {1,     1,     1,    .5,     0,     0,     0,     0,     0},
                    {1,    .5,     0,     0,     0,     0,     0,     0,     0},
                    {0,     0,     0,     0,     0,     0,     0,     0,     0}
            },
            {
                    {-.89,-.89, -.89,  -.89,  -.89,  -.89,  -.89,     1,     1},
                    {-.89,-.89, -.89,  -.89,  -.89,  -.89,     1,     1,     1},
                    {-.89,-.89, -.89,  -.89,  -.89,     1,     1,     1,     0},
                    {-.89,-.89, -.89,  -.89,     1,     1,     1,     0,     0},
                    {-.89,-.89, -.89,     1,     1,     1,     0,     0,     0},
                    {-.89,-.89,    1,     1,     1,     0,     0,     0,     0},
                    {-.89,  1,     1,     1,     0,     0,     0,     0,     0},
                    {1,     1,     1,     0,     0,     0,     0,     0,     0},
                    {1,     1,     0,     0,     0,     0,     0,     0,     0}
            }
    };

    Mat kernel1(9, 9, CV_32FC1, karray[0]);
    kernel1 /= 27;
    Mat kernel2(9, 9, CV_32FC1, karray[1]);
    kernel2 /= 25;
    Mat kernel3(9, 9, CV_32FC1, karray[2]);
    kernel3 /= 25;

    Mat kernel4 = kernel2.t();
    Mat kernel5 = kernel1.t();

    Mat kernel6;
    Mat kernel7;
    Mat kernel8;

    flip(kernel4, kernel6, 0);
    flip(kernel3, kernel7, 0);
    flip(kernel2, kernel8, 0);

    kernels[0] = kernel1.clone();
    kernels[1] = kernel2.clone();
    kernels[2] = kernel3.clone();
    kernels[3] = kernel4.clone();
    kernels[4] = kernel5.clone();
    kernels[5] = kernel6.clone();
    kernels[6] = kernel7.clone();
    kernels[7] = kernel8.clone();

    for (int i = 0; i < KERNAL_COUNT; i++) {
        Mat kernelComplement;
        flip(kernels[i], kernelComplement, -1);
        kernelComplements[i] = kernelComplement.clone();
    }
}

void LineDetector::DetectLines(int lineThickness) {

    // Resize the image such that the lines are approximately 3 pixels wide
    //cerr << "DetectLines::Reducing Image" << endl;
    lineThickness = max(1, lineThickness); // 0 thickness doesn't make sense
    resize(src_img, working, Size(3*src_img.cols/lineThickness, 3*src_img.rows/lineThickness), 0, 0, INTER_LANCZOS4);

    // Convert the image into HSV space to make processing white lines easier
    //cerr << "DetectLines::Converting to HSV" << endl;
    cvtColor(working, working, CV_BGR2HSV);

    // Calculate each pixel's "whiteness" defined as value*(255-saturation);
    //cerr << "DetectLines::Filtering Whiteness" << endl;
    WhitenessFilter(working, working);

    // Pass directional kernels over image
    //cerr << "DetectLines::Filtering kernels" << endl;
    for(size_t i = 0; i < KERNAL_COUNT; i++)
        filter2D(working, kernelResults[i], -1, kernels[i]);

    // Pass directional kernel complements over image (same edge, rotated 180 degrees, 3 pixels between edge and complement)
    //cerr << "DetectLines::Filtering complements" << endl;
    for(size_t i = 0; i < KERNAL_COUNT; i++)
        filter2D(working, complementResults[i], -1, kernelComplements[i]);

    // Multiply the results of kernel filter by its complement
    //cerr << "DetectLines::Multiplying Results" << endl;
    MultiplyByComplements(kernelResults, complementResults, kernelResults);

    working = Mat::zeros(kernelResults[0].size(), kernelResults[0].type());
    //cerr << "DetectLines::Thresholding Results" << endl;
    for(int i = 0; i < KERNAL_COUNT; i++) {
        // threshold(kernelResults[i], kernelResults[i], ((float)lineContinue*lineContinue)/255, 255, CV_THRESH_BINARY);
        adaptiveThreshold(kernelResults[i], kernelResults[i], 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 9, -1.85);
        bitwise_or(working, kernelResults[i], working);
    }
}

void LineDetector::WhitenessFilter(Mat& hsv_image, Mat& fin_img) {
    Mat result = 255 * Mat::ones(hsv_image.size(), CV_16UC1);
    Mat tmp;
    hsv_image.convertTo(tmp, CV_16UC3, 1.0);
    Mat channel[3];
    split(tmp, channel);
    result = result - channel[1];
    result = result.mul(channel[2]);
    result.convertTo(fin_img, CV_8UC1, 1.0/255);
}

void LineDetector::MultiplyByComplements(Mat* images, Mat* complements, Mat* results) {
    for(size_t i = 0; i < KERNAL_COUNT; i++) {
        Mat image;
        Mat complement;
        Mat result = Mat::zeros(images[0].size(), CV_16UC1);
        images[i].convertTo(image, CV_16UC1, 1.0);
        complements[i].convertTo(complement, CV_16UC1, 1.0);
        result = image.mul(complement);
        result.convertTo(results[i], CV_8UC1, 1.0/255); // TODO figure this const out
    }
}

void LineDetector::EnforceLength(Mat& img, int length) {
    // Thresholding the line length using the area of the contours
    vector<vector<Point>> contours;
    vector<vector<Point>> contoursThreshold;
    vector<Vec4i> hierarchy;

    Mat copy = img.clone();
    findContours(copy, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

    for (unsigned int i = 0; i < contours.size(); ++i) {
        if ((int) contours[i].size() < length) {
            contoursThreshold.push_back(contours[i]);
        }
    }

    Scalar color(0, 0, 0);
    drawContours(img, contoursThreshold, -1, color, 3);
}
<|endoftext|>