<commit_before>#include <iostream>

#include <opencv2/opencv.hpp>

#include <deque>

#include "sensors_drivers/logparser.h"

#include "data_manipulation/poleextractor.h"
#include "data_manipulation/lineextractor.h"

#include "operations/motionoperation.h"

#include "utils/gui.h"

//#define LINE_FOLLOWER_
//#define EGOMOTION_ESTIMATION_

void help();
float runningAverage(std::deque<float> &queue, const float value);

//////
/// RANDOM NUMBER GENERATOR
const double DBL_EPS_COMP = 1 - DBL_EPSILON; // DBL_EPSILON is defined in <limits.h>.
inline double RandU() {
    return DBL_EPSILON + ((double) rand()/RAND_MAX);
}
inline double RandN2(double mu, double sigma) {
    return mu + (rand()%2 ? -1.0 : 1.0)*sigma*pow(-log(DBL_EPS_COMP*RandU()), 0.5);
}
inline double RandN() {
    return RandN2(0, 0.1);
}
///
//////

int main(int argc, char **argv)
{
    //std::cout << "Hi" << std::endl;

    //////
    /// Check arguments and set the locale
    if (argc != 3)
    {
        help();
    }

    if (std::string(argv[1]) != "-s")
    {
        help();
    }

    std::cout << std::fixed << std::setprecision(6);
    setlocale(LC_NUMERIC, "C");

    //////
    /// Open config file
    std::string
        settfilename = argv[2];

    cv::FileStorage
        fs;

    fs.open(settfilename, cv::FileStorage::READ);

    if (!fs.isOpened())
    {
        std::cerr << "Could not open settings file: " << settfilename << std::endl;
        exit(-2);
    }


    int waitkey;
    fs["logparser"]["waitkey"] >> waitkey;
    int minLineSize;
    fs["lineExtractor"]["minLineSize"] >> minLineSize;
    float minHeadPoleDistance;
    fs["lineFollower"]["minHeadPoleDistance"] >> minHeadPoleDistance;

    /// Setup the GUI
    std::shared_ptr<nav::gui> GUI = std::make_shared<nav::gui>(fs, "Navigation GUI", waitkey);
    char c = 0; // waitkey char

    std::string operation;
    fs["logparser"]["operation"] >> operation;

    std::deque<float> leftVel, rightVel;
    float maxV;
    fs["globalMP"]["maxV"] >> maxV;


    std::shared_ptr<nav::MotionOperation> mo;
    bool initialized = false;
    if (operation.compare("001L") == 0)
    {
        mo = std::make_shared<nav::LineFollowerMO>(fs, false, GUI);
    }
    else if (operation.compare("001R") == 0)
    {
        mo = std::make_shared<nav::LineFollowerMO>(fs, true, GUI);
    }
    else if (operation.compare("003L") == 0)
    {
        mo = std::make_shared<nav::TurnWithCompassMO>(fs, false, GUI);
    }
    else if (operation.compare("003R") == 0)
    {
        mo = std::make_shared<nav::TurnWithCompassMO>(fs, true, GUI);
    }

    nav::Control control = {0.0f, 0.0f};

    std::vector<nav::Frame> framesVector;

    nav::parseFile(fs, framesVector);

    vineyard::PoleExtractor pe(fs);
    std::shared_ptr< std::vector< vineyard::Pole::Ptr > > polesVector;

    for (nav::Frame f : framesVector)
    {
        GUI->refresh();

        GUI->drawHUD(f.frameID);
        GUI->drawCompass(f.bearing);

        pcl::PointCloud<pcl::PointXYZ>::Ptr
                tempCloud(new pcl::PointCloud<pcl::PointXYZ>());
        for (nav::PT pt : f.points)
        {
            pcl::PointXYZ current;
            current.x = pt.scan_pt.x;
            current.y = pt.scan_pt.y;
            current.z = 0;

            tempCloud->points.push_back(current);
        }
        tempCloud->width = (int) tempCloud->points.size ();
        tempCloud->height = 1;

        pe.elaborateCloud(tempCloud, polesVector);

        std::vector<cv::Point2f> ptVector;
        for (nav::PT pt : f.points)
        {
            ptVector.push_back(pt.scan_pt);
        }

        GUI->drawPoints(ptVector);
        GUI->drawPoles(*polesVector);

        if (f.oper_t.compare("003R") != 0 && f.oper_t.compare("003L") != 0 &&
            f.oper_t.compare("001R") != 0 && f.oper_t.compare("001L") != 0 || f.frameID <= 2200)
        {
            continue;
        }

        if ((operation.compare("001L") == 0 || operation.compare("001R") == 0))
        {
            std::shared_ptr<nav::LineFollowerMO> lfmo = std::static_pointer_cast<nav::LineFollowerMO>(mo);

            lfmo->updateParameters(polesVector,
                                   control,
                                   f.bearing,
                                   90*M_PI/180);

            //std::cout << "bearing " << f.bearing << std::endl;

            control = lfmo->computeOperationControl();

            if (lfmo->checkOperationEnd())
            {
                float exitLineAngle;
                cv::Point2f headPole;
                lfmo->getFinalStatus(exitLineAngle, headPole);

                cv::waitKey();
                return 0;
            }
        }
        if ((operation.compare("003L") == 0 || operation.compare("003R") == 0))
        {
            std::shared_ptr<nav::TurnWithCompassMO> twcmo = std::static_pointer_cast<nav::TurnWithCompassMO>(mo);

            cv::Point2f initialPolePosition(-0.58,1.29);
            float forwardDistance = 4.5f;
            float fixedTurnAngle = M_PI/2;
            float fixedTurnRadius = 2.9f;
            float lineAngle = 0.1f;
            if (!initialized)
            {
                twcmo->initialize(polesVector,
                                  f.bearing,
                                  initialPolePosition,
                                  lineAngle,
                                  forwardDistance,
                                  fixedTurnAngle,
                                  fixedTurnRadius);
                initialized = true;
            }
            else
            {
                twcmo->updateParameters(polesVector,<commit_msg>variable name consistency in the test code<commit_after>#include <iostream>

#include <opencv2/opencv.hpp>

#include <deque>

#include "sensors_drivers/logparser.h"

#include "data_manipulation/poleextractor.h"
#include "data_manipulation/lineextractor.h"

#include "operations/motionoperation.h"

#include "utils/gui.h"

//#define LINE_FOLLOWER_
//#define EGOMOTION_ESTIMATION_

void help();
float runningAverage(std::deque<float> &queue, const float value);

//////
/// RANDOM NUMBER GENERATOR
const double DBL_EPS_COMP = 1 - DBL_EPSILON; // DBL_EPSILON is defined in <limits.h>.
inline double RandU() {
    return DBL_EPSILON + ((double) rand()/RAND_MAX);
}
inline double RandN2(double mu, double sigma) {
    return mu + (rand()%2 ? -1.0 : 1.0)*sigma*pow(-log(DBL_EPS_COMP*RandU()), 0.5);
}
inline double RandN() {
    return RandN2(0, 0.1);
}
///
//////

int main(int argc, char **argv)
{
    //std::cout << "Hi" << std::endl;

    //////
    /// Check arguments and set the locale
    if (argc != 3)
    {
        help();
    }

    if (std::string(argv[1]) != "-s")
    {
        help();
    }

    std::cout << std::fixed << std::setprecision(6);
    setlocale(LC_NUMERIC, "C");

    //////
    /// Open config file
    std::string
        settfilename = argv[2];

    cv::FileStorage
        fs;

    fs.open(settfilename, cv::FileStorage::READ);

    if (!fs.isOpened())
    {
        std::cerr << "Could not open settings file: " << settfilename << std::endl;
        exit(-2);
    }


    int waitkey;
    fs["logparser"]["waitkey"] >> waitkey;
    int minLineSize;
    fs["lineExtractor"]["minLineSize"] >> minLineSize;
    float minHeadPoleDistance;
    fs["lineFollower"]["minHeadPoleDistance"] >> minHeadPoleDistance;

    /// Setup the GUI
    std::shared_ptr<nav::gui> GUI = std::make_shared<nav::gui>(fs, "Navigation GUI", waitkey);
    char c = 0; // waitkey char

    std::string operation;
    fs["logparser"]["operation"] >> operation;

    std::deque<float> leftVel, rightVel;
    float maxV;
    fs["globalMP"]["maxV"] >> maxV;


    std::shared_ptr<nav::MotionOperation> mo;
    bool initialized = false;
    if (operation.compare("001L") == 0)
    {
        mo = std::make_shared<nav::LineFollowerMO>(fs, false, GUI);
    }
    else if (operation.compare("001R") == 0)
    {
        mo = std::make_shared<nav::LineFollowerMO>(fs, true, GUI);
    }
    else if (operation.compare("003L") == 0)
    {
        mo = std::make_shared<nav::TurnWithCompassMO>(fs, false, GUI);
    }
    else if (operation.compare("003R") == 0)
    {
        mo = std::make_shared<nav::TurnWithCompassMO>(fs, true, GUI);
    }

    nav::Control control = {0.0f, 0.0f};

    std::vector<nav::Frame> framesVector;

    nav::parseFile(fs, framesVector);

    vineyard::PoleExtractor pe(fs);
    std::shared_ptr< std::vector< vineyard::Pole::Ptr > > polesVector;

    for (nav::Frame f : framesVector)
    {
        GUI->refresh();

        GUI->drawHUD(f.frameID);
        GUI->drawCompass(f.bearing);

        pcl::PointCloud<pcl::PointXYZ>::Ptr
                tempCloud(new pcl::PointCloud<pcl::PointXYZ>());
        for (nav::PT pt : f.points)
        {
            pcl::PointXYZ current;
            current.x = pt.scan_pt.x;
            current.y = pt.scan_pt.y;
            current.z = 0;

            tempCloud->points.push_back(current);
        }
        tempCloud->width = (int) tempCloud->points.size ();
        tempCloud->height = 1;

        pe.elaborateCloud(tempCloud, polesVector);

        std::vector<cv::Point2f> ptVector;
        for (nav::PT pt : f.points)
        {
            ptVector.push_back(pt.scan_pt);
        }

        GUI->drawPoints(ptVector);
        GUI->drawPoles(*polesVector);

        if (f.oper_t.compare("003R") != 0 && f.oper_t.compare("003L") != 0 &&
            f.oper_t.compare("001R") != 0 && f.oper_t.compare("001L") != 0 || f.frameID <= 2200)
        {
            continue;
        }

        if ((operation.compare("001L") == 0 || operation.compare("001R") == 0))
        {
            std::shared_ptr<nav::LineFollowerMO> lfmo = std::static_pointer_cast<nav::LineFollowerMO>(mo);

            lfmo->updateParameters(polesVector,
                                   control,
                                   f.bearing,
                                   90*M_PI/180);

            //std::cout << "bearing " << f.bearing << std::endl;

            control = lfmo->computeOperationControl();

            if (lfmo->checkOperationEnd())
            {
                // messi qui solo per test -> devono essere variabili globali
                float lineAngle;
                cv::Point2f initialPolePosition;
                //

                lfmo->getFinalStatus(lineAngle, initialPolePosition);

                cv::waitKey();
                return 0;
            }
        }
        if ((operation.compare("003L") == 0 || operation.compare("003R") == 0))
        {
            std::shared_ptr<nav::TurnWithCompassMO> twcmo = std::static_pointer_cast<nav::TurnWithCompassMO>(mo);

            // messi qui solo per test -> devono essere variabili globali
            cv::Point2f initialPolePosition(-0.58,1.29);    // da linefollower
            float lineAngle = 0.1f;                         // da linefollower
            float forwardDistance = 4.5f;   // da setup
            float fixedTurnAngle = M_PI/2;  // da setup
            float fixedTurnRadius = 1.45f;   // da setup
            //

            if (!initialized)
            {
                twcmo->initialize(polesVector,
                                  f.bearing,
                                  initialPolePosition,
                                  lineAngle,
                                  forwardDistance,
                                  fixedTurnAngle,
                                  fixedTurnRadius);
                initialized = true;
            }
            else
            {
                twcmo->updateParameters(polesVector,
{'fraction_non_alphanumeric': 0.14752969530039595, 'fraction_numerical': 0.017300740230676535, 'mean_word_length': 1.9670582226762001, 'pattern_counts': {'":': 0, '<': 91, '<?xml version=': 0, '>': 101, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 18, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3904258', 'n_tokens_mistral': 3903, 'n_tokens_neox': 3620, 'n_words': 665}