<commit_before>#include "itkImage.h"
#include "itkRandomImageSource.h"
#include "itkGradientRecursiveGaussianImageFilter.h"
#include "itkGradientMagnitudeImageFilter.h"
#include "itkImageFileReader.h"

#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>
#include <vtkActor.h>
#include <vtkSmartPointer.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper2D.h>

#include <opencv2/core.hpp>
#include <iostream>

namespace
{
typedef itk::Image< unsigned char, 2 >    ImageType;
typedef itk::Image< float,  2 >           FloatImageType;
typedef ImageType::IndexType              IndexType;
typedef itk::CovariantVector< float, 2  > OutputPixelType;
typedef itk::Image< OutputPixelType, 2 >  OutputImageType;
typedef itk::GradientRecursiveGaussianImageFilter<
FloatImageType, OutputImageType>        FilterType;
typedef itk::GradientMagnitudeImageFilter<
ImageType, FloatImageType >             GradMagfilterType;
typedef itk::ImageFileReader< ImageType > ReaderType;
}

cv::Mat1d generateCircle( double cx, double cy, double rx, double ry, int n);
void createImage(ImageType::Pointer image,
                 int w, int h, double cx, double cy, double rx, double ry);
cv::Mat1d computeP(double alpha, double beta, double gamma, double N) throw (int);
cv::Mat1d sampleImage(cv::Mat1d x, cv::Mat1d y, OutputImageType::Pointer gradient, int position);

vtkSmartPointer<vtkPolyData> createPolydataLine( cv::Mat1d const & v )
{
    // Create a vtkPoints object and store the points in it
    vtkSmartPointer<vtkPoints> points =
            vtkSmartPointer<vtkPoints>::New();
    for( int i = 0; i < v.rows; ++i )
    {
        points->InsertNextPoint( v(2*i+0), v(2*i+1), 0 );
    }

    vtkSmartPointer<vtkPolyLine> polyLine =
            vtkSmartPointer<vtkPolyLine>::New();
    polyLine->GetPointIds()->SetNumberOfIds(v.rows);
    for(unsigned int i = 0; i < v.rows; i++)
    {
        polyLine->GetPointIds()->SetId(i,i);
    }

    // Create a cell array to store the lines in and add the lines to it
    vtkSmartPointer<vtkCellArray> cells =
            vtkSmartPointer<vtkCellArray>::New();
    cells->InsertNextCell( polyLine );

    // Create a polydata to store everything in
    vtkSmartPointer<vtkPolyData> polyData =
            vtkSmartPointer<vtkPolyData>::New();

    // Add the points to the dataset
    polyData->SetPoints(points);

    // Add the lines to the dataset
    polyData->SetLines(cells);

    return polyData;
}

int main( int argc, char* argv[] )
{
    //Image dimensions
    int w = 300;
    int h = 300;
    ImageType::Pointer image;
    if (argc < 7)
    {
        std::cout << "Usage " << argv[0]
                  << " points alpha beta gamma sigma iterations [image]"
                  << std::endl;
        return EXIT_SUCCESS;;
    }
    else if (argc < 8)
    {
        //Synthesize the image
        image = ImageType::New();
        createImage(image, w, h, 150, 150, 50, 50);
    }
    else if (argc == 8)
    {
        //Open the image
        ReaderType::Pointer reader = ReaderType::New();
        reader->SetFileName( argv[7] );
        try
        {
            reader->Update();
            image = reader->GetOutput();
            w = image->GetLargestPossibleRegion().GetSize()[0];
            h = image->GetLargestPossibleRegion().GetSize()[1];
        }
        catch( itk::ExceptionObject & err )
        {
            std::cerr << "Caught unexpected exception " << err;
            return EXIT_FAILURE;
        }
    }

    //Snake parameters
    double alpha = 0.001;
    double beta = 0.4;
    double gamma = 100;
    double iterations = 1;
    int nPoints = 20;
    double sigma;

    nPoints = atoi(argv[1]);
    alpha = atof(argv[2]);
    beta = atof(argv[3]);
    gamma = atof(argv[4]);
    sigma = atof(argv[5]);
    iterations = atoi(argv[6]);

    //Temporal variables
    cv::Mat1d P;
    cv::Mat1d v;
    double N;

    //Generate initial snake circle
    v = generateCircle(130, 130, 50, 50, nPoints);

    vtkSmartPointer<vtkPolyData> polyData =
            createPolydataLine( v );

    // Setup actor and mapper
    vtkSmartPointer<vtkPolyDataMapper2D> mapper =
            vtkSmartPointer<vtkPolyDataMapper2D>::New();
    mapper->SetInputData(polyData);

    vtkSmartPointer<vtkActor> actor =
            vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(mapper);

    // Setup render window, renderer, and interactor
    vtkSmartPointer<vtkRenderer> renderer =
            vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkRenderWindow> renderWindow =
            vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
            vtkSmartPointer<vtkRenderWindowInteractor>::New();
    renderWindowInteractor->SetRenderWindow(renderWindow);
    renderer->AddActor(actor);

    renderWindow->Render();

    //Computes P matrix.
    N = v.rows/2;
    try
    {
        P = computeP(alpha, beta, gamma, N);
    }
    catch (int n)
    {
        return EXIT_FAILURE;;
    }

    //Computes the magnitude gradient
    GradMagfilterType::Pointer gradientMagnitudeFilter =
            GradMagfilterType::New();
    gradientMagnitudeFilter->SetInput( image );
    gradientMagnitudeFilter->Update();

    //Computes the gradient of the gradient magnitude
    FilterType::Pointer gradientFilter = FilterType::New();
    gradientFilter->SetInput( gradientMagnitudeFilter->GetOutput() );
    gradientFilter->SetSigma( sigma );
    gradientFilter->Update();

    //Loop
    cv::Mat1d x(N,1);
    cv::Mat1d y(N,1);

    std::cout << "Initial snake" << std::endl;
    for (int i = 0; i < N; i++)
    {
        x(i) = v(2*i);
        y(i) = v(2*i+1);
        std::cout << "(" << x(i) << ", " << y(i) << ")" << std::endl;
    }

    for (int i = 0; i < iterations; i++)
    {

        x = (x+gamma*sampleImage(x, y, gradientFilter->GetOutput(), 0));
        x = (x.t() * P).t();
        y = (y+gamma*sampleImage(x, y, gradientFilter->GetOutput(), 1));
        y = ( y.t() * P ).t();
    }

    //Display the answer
    std::cout << "Final snake after " << iterations << " iterations" << std::endl;
    cv::Mat1d v2(2*N,1);
    for (int i=0; i<N; i++)
    {
        v2(2*i) = x(i);
        v2(2*i+1) = y(i);
        std::cout << "(" << x(i) << ", " << y(i) << ")" << std::endl;
    }

    return EXIT_SUCCESS;;
}

cv::Mat1d generateCircle( double cx, double cy, double rx, double ry, int n)
{
    cv::Mat1d v(2*(n+1),1);

    for (int i=0; i<n; i++)
    {
        v(2*i) = cx + rx*cos(2*M_PI*i/n);
        v(2*i+1) = cy + ry*sin(2*M_PI*i/n);
    }
    v(2*n)=v(0);
    v(2*n+1)=v(1);
    return v;
}

void createImage(ImageType::Pointer image,
                 int w, int h, double cx, double cy, double rx, double ry)
{

    itk::Size<2> size;
    size[0] = w;
    size[1] = h;

    itk::RandomImageSource<ImageType>::Pointer randomImageSource = itk::RandomImageSource<ImageType>::New();
    randomImageSource->SetNumberOfThreads(1); // to produce non-random results
    randomImageSource->SetSize(size);
    randomImageSource->SetMin(200);
    randomImageSource->SetMax(255);
    randomImageSource->Update();

    image->SetRegions(randomImageSource->GetOutput()->GetLargestPossibleRegion());
    image->Allocate();

    IndexType index;

    //Draw oval.
    for (int i=0; i<w; i++)
    {
        for (int j=0; j<h; j++)
        {
            index[0] = i; index[1] = j;
            if ( ((i-cx)*(i-cx)/(rx*rx) + (j-cy)*(j-cy)/(ry*ry) ) < 1)
            {
                image->SetPixel(index, randomImageSource->GetOutput()->GetPixel(index)-100);
            }
            else
            {
                image->SetPixel(index, randomImageSource->GetOutput()->GetPixel(index));
            }

        }
    }
}

cv::Mat1d computeP(double alpha, double beta, double gamma, double N) throw (int)
{

    double a = gamma*(2*alpha+6*beta)+1;
    double b = gamma*(-alpha-4*beta);
    double c = gamma*beta;

    cv::Mat1d P( N, N, 0.0 );

    //fill diagonal
    cv::Mat1d diag = P.diag();
    diag.setTo( cv::Scalar( a ) );

    //fill next two diagonals
    for (int i=0; i<(N-1); i++)
    {
        P(i+1,i) = b;
        P(i,i+1) = b;
    }
    //Moreover
    P(0, N-1)=b;
    P(N-1, 0)=b;

    //fill next two diagonals
    for (int i=0; i<(N-2); i++)
    {
        P(i+2,i) = c;
        P(i,i+2) = c;
    }
    //Moreover
    P(0, N-2)=c;
    P(1, N-1)=c;
    P(N-2, 0)=c;
    P(N-1, 1)=c;

    if ( cv::determinant( P ) == 0.0 )
    {
        std::cerr << "Singular matrix. Determinant is 0." << std::endl;
        throw 2;
    }

    //Compute the inverse of the matrix P
    cv::Mat1d Pinv;
    Pinv = P.inv();

    return Pinv.t();
}

cv::Mat1d sampleImage(cv::Mat1d x, cv::Mat1d y, OutputImageType::Pointer gradient, int position)
{
    int size;
    size = x.rows;
    cv::Mat1d ans(size,1);

    IndexType index;
    for (int i=0; i<size; i++)
    {
        index[0] = x(i);
        index[1] = y(i);
        ans(i) = gradient->GetPixel(index)[position];
    }
    return ans;
}
<commit_msg>#8 fixed M_PI for Windows<commit_after>#include "itkImage.h"
#include "itkRandomImageSource.h"
#include "itkGradientRecursiveGaussianImageFilter.h"
#include "itkGradientMagnitudeImageFilter.h"
#include "itkImageFileReader.h"

#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkRenderer.h>
#include <vtkActor.h>
#include <vtkSmartPointer.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper2D.h>

#include <opencv2/core.hpp>
#include <iostream>

#ifndef M_PI
#define M_PI		3.14159265358979323846	/* pi */
#endif // M_PI

namespace
{
typedef itk::Image< unsigned char, 2 >    ImageType;
typedef itk::Image< float,  2 >           FloatImageType;
typedef ImageType::IndexType              IndexType;
typedef itk::CovariantVector< float, 2  > OutputPixelType;
typedef itk::Image< OutputPixelType, 2 >  OutputImageType;
typedef itk::GradientRecursiveGaussianImageFilter<
FloatImageType, OutputImageType>        FilterType;
typedef itk::GradientMagnitudeImageFilter<
ImageType, FloatImageType >             GradMagfilterType;
typedef itk::ImageFileReader< ImageType > ReaderType;
}

cv::Mat1d generateCircle( double cx, double cy, double rx, double ry, int n);
void createImage(ImageType::Pointer image,
                 int w, int h, double cx, double cy, double rx, double ry);
cv::Mat1d computeP(double alpha, double beta, double gamma, double N) throw (int);
cv::Mat1d sampleImage(cv::Mat1d x, cv::Mat1d y, OutputImageType::Pointer gradient, int position);

vtkSmartPointer<vtkPolyData> createPolydataLine( cv::Mat1d const & v )
{
    // Create a vtkPoints object and store the points in it
    vtkSmartPointer<vtkPoints> points =
            vtkSmartPointer<vtkPoints>::New();
    for( int i = 0; i < v.rows; ++i )
    {
        points->InsertNextPoint( v(2*i+0), v(2*i+1), 0 );
    }

    vtkSmartPointer<vtkPolyLine> polyLine =
            vtkSmartPointer<vtkPolyLine>::New();
    polyLine->GetPointIds()->SetNumberOfIds(v.rows);
    for(unsigned int i = 0; i < v.rows; i++)
    {
        polyLine->GetPointIds()->SetId(i,i);
    }

    // Create a cell array to store the lines in and add the lines to it
    vtkSmartPointer<vtkCellArray> cells =
            vtkSmartPointer<vtkCellArray>::New();
    cells->InsertNextCell( polyLine );

    // Create a polydata to store everything in
    vtkSmartPointer<vtkPolyData> polyData =
            vtkSmartPointer<vtkPolyData>::New();

    // Add the points to the dataset
    polyData->SetPoints(points);

    // Add the lines to the dataset
    polyData->SetLines(cells);

    return polyData;
}

int main( int argc, char* argv[] )
{
    //Image dimensions
    int w = 300;
    int h = 300;
    ImageType::Pointer image;
    if (argc < 7)
    {
        std::cout << "Usage " << argv[0]
                  << " points alpha beta gamma sigma iterations [image]"
                  << std::endl;
        return EXIT_SUCCESS;;
    }
    else if (argc < 8)
    {
        //Synthesize the image
        image = ImageType::New();
        createImage(image, w, h, 150, 150, 50, 50);
    }
    else if (argc == 8)
    {
        //Open the image
        ReaderType::Pointer reader = ReaderType::New();
        reader->SetFileName( argv[7] );
        try
        {
            reader->Update();
            image = reader->GetOutput();
            w = image->GetLargestPossibleRegion().GetSize()[0];
            h = image->GetLargestPossibleRegion().GetSize()[1];
        }
        catch( itk::ExceptionObject & err )
        {
            std::cerr << "Caught unexpected exception " << err;
            return EXIT_FAILURE;
        }
    }

    //Snake parameters
    double alpha = 0.001;
    double beta = 0.4;
    double gamma = 100;
    double iterations = 1;
    int nPoints = 20;
    double sigma;

    nPoints = atoi(argv[1]);
    alpha = atof(argv[2]);
    beta = atof(argv[3]);
    gamma = atof(argv[4]);
    sigma = atof(argv[5]);
    iterations = atoi(argv[6]);

    //Temporal variables
    cv::Mat1d P;
    cv::Mat1d v;
    double N;

    //Generate initial snake circle
    v = generateCircle(130, 130, 50, 50, nPoints);

    vtkSmartPointer<vtkPolyData> polyData =
            createPolydataLine( v );

    // Setup actor and mapper
    vtkSmartPointer<vtkPolyDataMapper2D> mapper =
            vtkSmartPointer<vtkPolyDataMapper2D>::New();
    mapper->SetInputData(polyData);

    vtkSmartPointer<vtkActor> actor =
            vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(mapper);

    // Setup render window, renderer, and interactor
    vtkSmartPointer<vtkRenderer> renderer =
            vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkRenderWindow> renderWindow =
            vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
            vtkSmartPointer<vtkRenderWindowInteractor>::New();
    renderWindowInteractor->SetRenderWindow(renderWindow);
    renderer->AddActor(actor);

    renderWindow->Render();

    //Computes P matrix.
    N = v.rows/2;
    try
    {
        P = computeP(alpha, beta, gamma, N);
    }
    catch (int n)
    {
        return EXIT_FAILURE;;
    }

    //Computes the magnitude gradient
    GradMagfilterType::Pointer gradientMagnitudeFilter =
            GradMagfilterType::New();
    gradientMagnitudeFilter->SetInput( image );
    gradientMagnitudeFilter->Update();

    //Computes the gradient of the gradient magnitude
    FilterType::Pointer gradientFilter = FilterType::New();
    gradientFilter->SetInput( gradientMagnitudeFilter->GetOutput() );
    gradientFilter->SetSigma( sigma );
    gradientFilter->Update();

    //Loop
    cv::Mat1d x(N,1);
    cv::Mat1d y(N,1);

    std::cout << "Initial snake" << std::endl;
    for (int i = 0; i < N; i++)
    {
        x(i) = v(2*i);
        y(i) = v(2*i+1);
        std::cout << "(" << x(i) << ", " << y(i) << ")" << std::endl;
    }

    for (int i = 0; i < iterations; i++)
    {

        x = (x+gamma*sampleImage(x, y, gradientFilter->GetOutput(), 0));
        x = (x.t() * P).t();
        y = (y+gamma*sampleImage(x, y, gradientFilter->GetOutput(), 1));
        y = ( y.t() * P ).t();
    }

    //Display the answer
    std::cout << "Final snake after " << iterations << " iterations" << std::endl;
    cv::Mat1d v2(2*N,1);
    for (int i=0; i<N; i++)
    {
        v2(2*i) = x(i);
        v2(2*i+1) = y(i);
        std::cout << "(" << x(i) << ", " << y(i) << ")" << std::endl;
    }

    return EXIT_SUCCESS;;
}

cv::Mat1d generateCircle( double cx, double cy, double rx, double ry, int n)
{
    cv::Mat1d v(2*(n+1),1);

    for (int i=0; i<n; i++)
    {
        v(2*i) = cx + rx*cos(2*M_PI*i/n);
        v(2*i+1) = cy + ry*sin(2*M_PI*i/n);
    }
    v(2*n)=v(0);
    v(2*n+1)=v(1);
    return v;
}

void createImage(ImageType::Pointer image,
                 int w, int h, double cx, double cy, double rx, double ry)
{

    itk::Size<2> size;
    size[0] = w;
    size[1] = h;

    itk::RandomImageSource<ImageType>::Pointer randomImageSource = itk::RandomImageSource<ImageType>::New();
    randomImageSource->SetNumberOfThreads(1); // to produce non-random results
    randomImageSource->SetSize(size);
    randomImageSource->SetMin(200);
    randomImageSource->SetMax(255);
    randomImageSource->Update();

    image->SetRegions(randomImageSource->GetOutput()->GetLargestPossibleRegion());
    image->Allocate();

    IndexType index;

    //Draw oval.
    for (int i=0; i<w; i++)
    {
        for (int j=0; j<h; j++)
        {
            index[0] = i; index[1] = j;
            if ( ((i-cx)*(i-cx)/(rx*rx) + (j-cy)*(j-cy)/(ry*ry) ) < 1)
            {
                image->SetPixel(index, randomImageSource->GetOutput()->GetPixel(index)-100);
            }
            else
            {
                image->SetPixel(index, randomImageSource->GetOutput()->GetPixel(index));
            }

        }
    }
}

cv::Mat1d computeP(double alpha, double beta, double gamma, double N) throw (int)
{

    double a = gamma*(2*alpha+6*beta)+1;
    double b = gamma*(-alpha-4*beta);
    double c = gamma*beta;

    cv::Mat1d P( N, N, 0.0 );

    //fill diagonal
    cv::Mat1d diag = P.diag();
    diag.setTo( cv::Scalar( a ) );

    //fill next two diagonals
    for (int i=0; i<(N-1); i++)
    {
        P(i+1,i) = b;
        P(i,i+1) = b;
    }
    //Moreover
    P(0, N-1)=b;
    P(N-1, 0)=b;

    //fill next two diagonals
    for (int i=0; i<(N-2); i++)
    {
        P(i+2,i) = c;
        P(i,i+2) = c;
    }
    //Moreover
    P(0, N-2)=c;
    P(1, N-1)=c;
    P(N-2, 0)=c;
    P(N-1, 1)=c;

    if ( cv::determinant( P ) == 0.0 )
    {
        std::cerr << "Singular matrix. Determinant is 0." << std::endl;
        throw 2;
    }

    //Compute the inverse of the matrix P
    cv::Mat1d Pinv;
    Pinv = P.inv();

    return Pinv.t();
}

cv::Mat1d sampleImage(cv::Mat1d x, cv::Mat1d y, OutputImageType::Pointer gradient, int position)
{
    int size;
    size = x.rows;
    cv::Mat1d ans(size,1);

    IndexType index;
    for (int i=0; i<size; i++)
    {
        index[0] = x(i);
        index[1] = y(i);
        ans(i) = gradient->GetPixel(index)[position];
    }
    return ans;
}
<|endoftext|>


<commit_before>#include "instruments/population/GenerationFitnessCSV.hpp"
#include <cmath>
#include <string>

GenerationFitnessCSV::GenerationFitnessCSV(
	PopulationNode* target,
	std::string outFile,
	float bottomFitness,
	float topFitness,
	float resolution
) : CSVInstrument(target, outFile) {
	std::vector<float> buckets;
	for (float i = bottomFitness; i <= topFitness; i+= resolution)
		buckets.push_back(i);
	this->buckets = buckets;
	this->resolution = resolution;
	this->addToHeader("Generation");
	this->addToHeader(this->buckets);
}

float GenerationFitnessCSV::bucket(float actual) {
	float mod = fmod(actual, this->resolution);

	return (mod < this->resolution/2 ?
		actual - mod : actual - mod + resolution);
}

std::map<std::string, unsigned int> GenerationFitnessCSV::buildEmptyMap() {
	std::map<std::string, unsigned int> counts;
	for (float fitness: this->buckets)
		counts.emplace(std::to_string(fitness), 0);
	return counts;
}

void GenerationFitnessCSV::reportFitnesses() {
	std::map<std::string, unsigned int> output = this->buildEmptyMap();
	output.emplace("Generation", this->target->currentGeneration());
	std::vector<float> fitnesses = this->target->getFitnesses();
	for (float fitness: fitnesses) output[std::to_string(fitness)]++;
	this->write(output, (unsigned int)0);
}

void GenerationFitnessCSV::initialReport() {
	this->reportFitnesses();
}

void GenerationFitnessCSV::runtimeReport() {
	this->reportFitnesses();
}

void GenerationFitnessCSV::endReport() {
	this->reportFitnesses();
}
<commit_msg>[GenerationFitnessCSV]: Fixed compile issues<commit_after>#include "instruments/population/GenerationFitnessCSV.hpp"
#include <cmath>
#include <string>

GenerationFitnessCSV::GenerationFitnessCSV(
	PopulationNode* target,
	std::string outFile,
	float bottomFitness,
	float topFitness,
	float resolution
) : CSVInstrument(target, outFile) {
	std::vector<float> buckets;
	std::vector<std::string> headerBuckets;
	for (float i = bottomFitness; i <= topFitness; i+= resolution) {
		buckets.push_back(i);
		headerBuckets.push_back(std::to_string(i));
	}
	this->buckets = buckets;
	this->resolution = resolution;
	this->addToHeader("Generation");
	this->addToHeader(headerBuckets);
}

float GenerationFitnessCSV::bucket(float actual) {
	float mod = fmod(actual, this->resolution);

	return (mod < this->resolution/2 ?
		actual - mod : actual - mod + resolution);
}

std::map<std::string, unsigned int> GenerationFitnessCSV::buildEmptyMap() {
	std::map<std::string, unsigned int> counts;
	for (float fitness: this->buckets)
		counts.emplace(std::to_string(fitness), 0);
	return counts;
}

void GenerationFitnessCSV::reportFitnesses() {
	std::map<std::string, unsigned int> output = this->buildEmptyMap();
	output.emplace("Generation", this->target->currentGeneration());
	std::vector<float> fitnesses = this->target->getFitnesses();
	for (float fitness: fitnesses) output[std::to_string(fitness)]++;
	this->write(output, (unsigned int)0);
}

void GenerationFitnessCSV::initialReport() {
	this->reportFitnesses();
}

void GenerationFitnessCSV::runtimeReport() {
	this->reportFitnesses();
}

void GenerationFitnessCSV::endReport() {
	this->reportFitnesses();
}
<|endoftext|>


<commit_before>inline
cov_mat_kth::cov_mat_kth( const std::string in_path,
			  const std::string in_actionNames,
			  const int in_scale_factor,
			  const int in_shift,
			  const int in_scene, //only for kth
			  const int in_segment_length
)
:path(in_path), actionNames(in_actionNames), scale_factor(in_scale_factor), shift(in_shift), scene(in_scene), segment_length(in_segment_length)
{
  actions.load( actionNames );
}

inline
void
cov_mat_kth::calculate( field<string> all_people, int  in_dim  )
{
  dim = in_dim;
  int n_actions = actions.n_rows;
  int n_peo =  all_people.n_rows;
  //all_people.print("people");

  for (int sc = 1; sc<=1; ++sc) //scene
  {
    for (int pe = 0; pe< n_peo; ++pe)
    {
      for (int act=0; act<n_actions; ++act)
      {

	std::stringstream load_folder;
	std::stringstream load_feat_video_i;
	std::stringstream load_labels_video_i;

	cout << "Loading.." << endl;
	load_folder << "./kth-features/sc" << sc << "/scale" << scale_factor << "-shift"<< shift ;
	load_feat_video_i << load_folder.str() << "/" << all_people (pe) << "_" << actions(act) << "_dim" << dim  << ".dat";
	load_labels_video_i << save_folder.str() << "/lab_" << all_people (pe) << "_" << actions(act) << "_dim" << dim  << ".dat";

	//For one Video
	cov_mat_kth::one_video(load_feat_video_i.str(),	 load_labels_video_i.str() );

      }
    }
  }

}

inline
void
cov_mat_kth::one_video( std::string load_feat_video_i,	std::string load_labels_video_i )
{
  mat mat_features_video_i;
  vec lab_video_i;

  mat_features_video_i.load( load_feat_video_i, raw_ascii );
  lab_video_i.load( load_labels_video_i, raw_ascii );
  vec n_vec = lab_video_i.n_elem;
  int last = lab_video_i( n_vec - 1 );

  for (int l=2; l<last-segment_length; l = l+4 )
  {
    running_stat_vec<rowvec> stat_seg(true);
    int k =0;

    for (int j=l; j<segment_length; ++j)
    {
      k++;
      uvec indices = find(lab_video_i == j);
      mat tmp_feat = mat_features_video_i.elem(indices);
      for (v=0; v < tmp_feat.n_cols; ++v)
      {
	vec sample = tmp_feat.col(v);
	stat_seg (sample);

      }

    }

   cout << "k= " << k << endl;
   getchar();

  }

}

/*
running_stat_vec<rowvec> more_stats(true);

for(uword i=0; i<20; ++i)
  {
  sample = randu<rowvec>(3);

  sample(1) -= sample(0);
  sample(2) += sample(1);

  more_stats(sample);
  }*/<commit_msg>First try<commit_after>inline
cov_mat_kth::cov_mat_kth( const std::string in_path,
			  const std::string in_actionNames,
			  const int in_scale_factor,
			  const int in_shift,
			  const int in_scene, //only for kth
			  const int in_segment_length
)
:path(in_path), actionNames(in_actionNames), scale_factor(in_scale_factor), shift(in_shift), scene(in_scene), segment_length(in_segment_length)
{
  actions.load( actionNames );
}

inline
void
cov_mat_kth::calculate( field<string> all_people, int  in_dim  )
{
  dim = in_dim;
  int n_actions = actions.n_rows;
  int n_peo =  all_people.n_rows;
  //all_people.print("people");

  for (int sc = 1; sc<=1; ++sc) //scene
  {
    for (int pe = 0; pe< n_peo; ++pe)
    {
      for (int act=0; act<n_actions; ++act)
      {

	std::stringstream load_folder;
	std::stringstream load_feat_video_i;
	std::stringstream load_labels_video_i;

	cout << "Loading.." << endl;
	load_folder << "./kth-features/sc" << sc << "/scale" << scale_factor << "-shift"<< shift ;
	load_feat_video_i << load_folder.str() << "/" << all_people (pe) << "_" << actions(act) << "_dim" << dim  << ".dat";
	load_labels_video_i << load_folder.str() << "/lab_" << all_people (pe) << "_" << actions(act) << "_dim" << dim  << ".dat";

	//For one Video
	cov_mat_kth::one_video(load_feat_video_i.str(),	 load_labels_video_i.str() );

      }
    }
  }

}

inline
void
cov_mat_kth::one_video( std::string load_feat_video_i,	std::string load_labels_video_i )
{
  mat mat_features_video_i;
  vec lab_video_i;

  mat_features_video_i.load( load_feat_video_i, raw_ascii );
  lab_video_i.load( load_labels_video_i, raw_ascii );
  vec n_vec = lab_video_i.n_elem;
  int last = lab_video_i( n_vec - 1 );

  for (int l=2; l<last-segment_length; l = l+4 )
  {
    running_stat_vec<rowvec> stat_seg(true);
    int k =0;

    for (int j=l; j<segment_length; ++j)
    {
      k++;
      uvec indices = find(lab_video_i == j);
      mat tmp_feat = mat_features_video_i.elem(indices);
      for (v=0; v < tmp_feat.n_cols; ++v)
      {
	vec sample = tmp_feat.col(v);
	stat_seg (sample);

      }

    }

   cout << "k= " << k << endl;
   getchar();

  }

}

/*
running_stat_vec<rowvec> more_stats(true);

for(uword i=0; i<20; ++i)
  {
  sample = randu<rowvec>(3);

  sample(1) -= sample(0);
  sample(2) += sample(1);

  more_stats(sample);
  }*/<|endoftext|>


<commit_before>// Copyright 2011 Software Freedom Conservatory
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "DocumentHost.h"
#include "Generated/cookies.h"
#include "logging.h"
#include "messages.h"

namespace webdriver {

DocumentHost::DocumentHost(HWND hwnd, HWND executor_handle) {
  // NOTE: COM should be initialized on this thread, so we
  // could use CoCreateGuid() and StringFromGUID2() instead.
  UUID guid;
  RPC_WSTR guid_string = NULL;
  RPC_STATUS status = ::UuidCreate(&guid);
  status = ::UuidToString(&guid, &guid_string);

  // RPC_WSTR is currently typedef'd in RpcDce.h (pulled in by rpc.h)
  // as unsigned short*. It needs to be typedef'd as wchar_t*
  wchar_t* cast_guid_string = reinterpret_cast<wchar_t*>(guid_string);
  this->browser_id_ = CW2A(cast_guid_string, CP_UTF8);

  ::RpcStringFree(&guid_string);

  this->window_handle_ = hwnd;
  this->executor_handle_ = executor_handle;
  this->is_closing_ = false;
  this->wait_required_ = false;
  this->focused_frame_window_ = NULL;
}

DocumentHost::~DocumentHost(void) {
}

std::string DocumentHost::GetCurrentUrl() {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  if (!doc) {
    LOG(WARN) << "Unable to get document object";
    return "";
  }

  CComBSTR url;
  HRESULT hr = doc->get_URL(&url);
  if (FAILED(hr)) {
    LOGHR(WARN, hr) << "Unable to get current URL";
    return "";
  }

  std::string current_url = CW2A(url, CP_UTF8);
  return current_url;
}

std::string DocumentHost::GetPageSource() {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  CComPtr<IHTMLDocument3> doc3;
  CComQIPtr<IHTMLDocument3> doc_qi_pointer(doc);
  if (doc_qi_pointer) {
    doc3 = doc_qi_pointer.Detach();
  }

  if (!doc3) {
    LOG(WARN) << "Unable to get document object";
    return "";
  }

  CComPtr<IHTMLElement> document_element;
  HRESULT hr = doc3->get_documentElement(&document_element);
  if (FAILED(hr)) {
    LOGHR(WARN, hr) << "Unable to get document element from page";
    return "";
  }

  CComBSTR html;
  hr = document_element->get_outerHTML(&html);
  if (FAILED(hr)) {
    LOGHR(WARN, hr) << "Have document element but cannot read source.";
    return "";
  }

  std::string page_source = CW2A(html, CP_UTF8);
  return page_source;
}

int DocumentHost::SetFocusedFrameByElement(IHTMLElement* frame_element) {
  HRESULT hr = S_OK;
  if (!frame_element) {
    this->focused_frame_window_ = NULL;
    return SUCCESS;
  }

  CComQIPtr<IHTMLFrameBase2> frame_base(frame_element);
  if (!frame_base) {
    // IHTMLElement is not a FRAME or IFRAME element.
    return ENOSUCHFRAME;
  }

  CComQIPtr<IHTMLWindow2> interim_result;
  hr = frame_base->get_contentWindow(&interim_result);
  if (FAILED(hr)) {
    // Cannot get contentWindow from IHTMLFrameBase2.
    return ENOSUCHFRAME;
  }

  this->focused_frame_window_ = interim_result;
  return SUCCESS;
}

int DocumentHost::SetFocusedFrameByName(const std::string& frame_name) {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  CComQIPtr<IHTMLFramesCollection2> frames;
  HRESULT hr = doc->get_frames(&frames);

  if (frames == NULL) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  long length = 0;
  frames->get_length(&length);
  if (!length) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  CComVariant name;
  CComBSTR name_bstr(CA2W(frame_name.c_str(), CP_UTF8));
  hr = name_bstr.CopyTo(&name);

  // Find the frame
  CComVariant frame_holder;
  hr = frames->item(&name, &frame_holder);

  if (FAILED(hr)) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  CComQIPtr<IHTMLWindow2> interim_result = frame_holder.pdispVal;
  if (!interim_result) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  this->focused_frame_window_ = interim_result;
  return SUCCESS;
}

int DocumentHost::SetFocusedFrameByIndex(const int frame_index) {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  CComQIPtr<IHTMLFramesCollection2> frames;
  HRESULT hr = doc->get_frames(&frames);

  if (frames == NULL) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  long length = 0;
  frames->get_length(&length);
  if (!length) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  CComVariant index;
  index.vt = VT_I4;
  index.lVal = frame_index;

  // Find the frame
  CComVariant frame_holder;
  hr = frames->item(&index, &frame_holder);

  if (FAILED(hr)) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  CComQIPtr<IHTMLWindow2> interim_result = frame_holder.pdispVal;
  if (!interim_result) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  this->focused_frame_window_ = interim_result;
  return SUCCESS;
}

void DocumentHost::GetCookies(std::map<std::string, std::string>* cookies) {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  if (!doc) {
    return;
  }

  CComBSTR cookie_bstr;
  HRESULT hr = doc->get_cookie(&cookie_bstr);
  if (!cookie_bstr) {
    cookie_bstr = L"";
  }

  std::wstring cookie_string = cookie_bstr;
  while (cookie_string.size() > 0) {
    size_t cookie_delimiter_pos = cookie_string.find(L"; ");
    std::wstring cookie = cookie_string.substr(0, cookie_delimiter_pos);
    if (cookie_delimiter_pos == std::wstring::npos) {
      cookie_string = L"";
    } else {
      cookie_string = cookie_string.substr(cookie_delimiter_pos + 2);
    }
    size_t cookie_separator_pos(cookie.find_first_of(L"="));
    std::string cookie_name(CW2A(cookie.substr(0, cookie_separator_pos).c_str(), CP_UTF8));
    std::string cookie_value(CW2A(cookie.substr(cookie_separator_pos + 1).c_str(), CP_UTF8));
    cookies->insert(std::pair<std::string, std::string>(cookie_name, cookie_value));
  }
}

int DocumentHost::AddCookie(const std::string& cookie) {
  CComBSTR cookie_bstr(CA2W(cookie.c_str(), CP_UTF8));

  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  if (!doc) {
    return EUNHANDLEDERROR;
  }

  if (!this->IsHtmlPage(doc)) {
    return ENOSUCHDOCUMENT;
  }

  if (!SUCCEEDED(doc->put_cookie(cookie_bstr))) {
    return EUNHANDLEDERROR;
  }

  return SUCCESS;
}

int DocumentHost::DeleteCookie(const std::string& cookie_name) {
  // Construct the delete cookie script
  std::wstring script_source;
  for (int i = 0; DELETECOOKIES[i]; i++) {
    script_source += DELETECOOKIES[i];
  }

  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);
  Script script_wrapper(doc, script_source, 1);
  script_wrapper.AddArgument(cookie_name);
  int status_code = script_wrapper.Execute();
  return status_code;
}

void DocumentHost::PostQuitMessage() {
  this->set_is_closing(true);
  LPSTR message_payload = new CHAR[this->browser_id_.size() + 1];
  strcpy_s(message_payload, this->browser_id_.size() + 1, this->browser_id_.c_str());
  ::PostMessage(this->executor_handle(),
                WD_BROWSER_QUIT,
                NULL,
                reinterpret_cast<LPARAM>(message_payload));
}

bool DocumentHost::IsHtmlPage(IHTMLDocument2* doc) {
  CComBSTR type;
  if (!SUCCEEDED(doc->get_mimeType(&type))) {
    return false;
  }

  std::wstring document_type_key_name= L"";
  if (this->factory_.GetRegistryValue(HKEY_CURRENT_USER,
                                      L"Software\\Microsoft\\Windows\\Shell\\Associations\\UrlAssociations\\http\\UserChoice",
                                      L"Progid",
                                      &document_type_key_name)) {
    // Look for the user-customization under Vista/Windows 7 first. If it's
    // IE, set the document friendly name lookup key to 'htmlfile'. If not,
    // set it to blank so that we can look up the proper HTML type.
    if (document_type_key_name == L"IE.HTTP") {
      document_type_key_name = L"htmlfile";
    } else {
      document_type_key_name = L"";
    }
  }

  if (document_type_key_name == L"") {
    // To be technically correct, we should look up the extension specified
    // for the text/html MIME type first (located in the "Extension" value
    // of HKEY_CLASSES_ROOT\MIME\Database\Content Type\text/html), but that
    // should always resolve to ".htm" anyway. From the extension, we can
    // find the browser-specific subkey of HKEY_CLASSES_ROOT, the default
    // value of which should contain the browser-specific friendly name of
    // the MIME type for HTML documents, which is what
    // IHTMLDocument2::get_mimeType() returns.
    if (!this->factory_.GetRegistryValue(HKEY_CLASSES_ROOT,
                                         L".htm",
                                         L"",
                                         &document_type_key_name)) {
      return false;
    }
  }

  std::wstring mime_type_name;
  if (!this->factory_.GetRegistryValue(HKEY_CLASSES_ROOT,
                                       document_type_key_name,
                                       L"",
                                       &mime_type_name)) {
    return false;
  }

  std::wstring type_string = type;
  return type_string == mime_type_name;
}

} // namespace webdriver<commit_msg>JimEvans: Changing DocumentHost::AddCookie to be slightly more readable. No functional changes.<commit_after>// Copyright 2011 Software Freedom Conservatory
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "DocumentHost.h"
#include "Generated/cookies.h"
#include "logging.h"
#include "messages.h"

namespace webdriver {

DocumentHost::DocumentHost(HWND hwnd, HWND executor_handle) {
  // NOTE: COM should be initialized on this thread, so we
  // could use CoCreateGuid() and StringFromGUID2() instead.
  UUID guid;
  RPC_WSTR guid_string = NULL;
  RPC_STATUS status = ::UuidCreate(&guid);
  status = ::UuidToString(&guid, &guid_string);

  // RPC_WSTR is currently typedef'd in RpcDce.h (pulled in by rpc.h)
  // as unsigned short*. It needs to be typedef'd as wchar_t*
  wchar_t* cast_guid_string = reinterpret_cast<wchar_t*>(guid_string);
  this->browser_id_ = CW2A(cast_guid_string, CP_UTF8);

  ::RpcStringFree(&guid_string);

  this->window_handle_ = hwnd;
  this->executor_handle_ = executor_handle;
  this->is_closing_ = false;
  this->wait_required_ = false;
  this->focused_frame_window_ = NULL;
}

DocumentHost::~DocumentHost(void) {
}

std::string DocumentHost::GetCurrentUrl() {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  if (!doc) {
    LOG(WARN) << "Unable to get document object";
    return "";
  }

  CComBSTR url;
  HRESULT hr = doc->get_URL(&url);
  if (FAILED(hr)) {
    LOGHR(WARN, hr) << "Unable to get current URL";
    return "";
  }

  std::string current_url = CW2A(url, CP_UTF8);
  return current_url;
}

std::string DocumentHost::GetPageSource() {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  CComPtr<IHTMLDocument3> doc3;
  CComQIPtr<IHTMLDocument3> doc_qi_pointer(doc);
  if (doc_qi_pointer) {
    doc3 = doc_qi_pointer.Detach();
  }

  if (!doc3) {
    LOG(WARN) << "Unable to get document object";
    return "";
  }

  CComPtr<IHTMLElement> document_element;
  HRESULT hr = doc3->get_documentElement(&document_element);
  if (FAILED(hr)) {
    LOGHR(WARN, hr) << "Unable to get document element from page";
    return "";
  }

  CComBSTR html;
  hr = document_element->get_outerHTML(&html);
  if (FAILED(hr)) {
    LOGHR(WARN, hr) << "Have document element but cannot read source.";
    return "";
  }

  std::string page_source = CW2A(html, CP_UTF8);
  return page_source;
}

int DocumentHost::SetFocusedFrameByElement(IHTMLElement* frame_element) {
  HRESULT hr = S_OK;
  if (!frame_element) {
    this->focused_frame_window_ = NULL;
    return SUCCESS;
  }

  CComQIPtr<IHTMLFrameBase2> frame_base(frame_element);
  if (!frame_base) {
    // IHTMLElement is not a FRAME or IFRAME element.
    return ENOSUCHFRAME;
  }

  CComQIPtr<IHTMLWindow2> interim_result;
  hr = frame_base->get_contentWindow(&interim_result);
  if (FAILED(hr)) {
    // Cannot get contentWindow from IHTMLFrameBase2.
    return ENOSUCHFRAME;
  }

  this->focused_frame_window_ = interim_result;
  return SUCCESS;
}

int DocumentHost::SetFocusedFrameByName(const std::string& frame_name) {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  CComQIPtr<IHTMLFramesCollection2> frames;
  HRESULT hr = doc->get_frames(&frames);

  if (frames == NULL) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  long length = 0;
  frames->get_length(&length);
  if (!length) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  CComVariant name;
  CComBSTR name_bstr(CA2W(frame_name.c_str(), CP_UTF8));
  hr = name_bstr.CopyTo(&name);

  // Find the frame
  CComVariant frame_holder;
  hr = frames->item(&name, &frame_holder);

  if (FAILED(hr)) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  CComQIPtr<IHTMLWindow2> interim_result = frame_holder.pdispVal;
  if (!interim_result) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  this->focused_frame_window_ = interim_result;
  return SUCCESS;
}

int DocumentHost::SetFocusedFrameByIndex(const int frame_index) {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  CComQIPtr<IHTMLFramesCollection2> frames;
  HRESULT hr = doc->get_frames(&frames);

  if (frames == NULL) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  long length = 0;
  frames->get_length(&length);
  if (!length) {
    // No frames in document. Exit.
    return ENOSUCHFRAME;
  }

  CComVariant index;
  index.vt = VT_I4;
  index.lVal = frame_index;

  // Find the frame
  CComVariant frame_holder;
  hr = frames->item(&index, &frame_holder);

  if (FAILED(hr)) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  CComQIPtr<IHTMLWindow2> interim_result = frame_holder.pdispVal;
  if (!interim_result) {
    // Error retrieving frame. Exit.
    return ENOSUCHFRAME;
  }

  this->focused_frame_window_ = interim_result;
  return SUCCESS;
}

void DocumentHost::GetCookies(std::map<std::string, std::string>* cookies) {
  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  if (!doc) {
    return;
  }

  CComBSTR cookie_bstr;
  HRESULT hr = doc->get_cookie(&cookie_bstr);
  if (!cookie_bstr) {
    cookie_bstr = L"";
  }

  std::wstring cookie_string = cookie_bstr;
  while (cookie_string.size() > 0) {
    size_t cookie_delimiter_pos = cookie_string.find(L"; ");
    std::wstring cookie = cookie_string.substr(0, cookie_delimiter_pos);
    if (cookie_delimiter_pos == std::wstring::npos) {
      cookie_string = L"";
    } else {
      cookie_string = cookie_string.substr(cookie_delimiter_pos + 2);
    }
    size_t cookie_separator_pos(cookie.find_first_of(L"="));
    std::string cookie_name(CW2A(cookie.substr(0, cookie_separator_pos).c_str(), CP_UTF8));
    std::string cookie_value(CW2A(cookie.substr(cookie_separator_pos + 1).c_str(), CP_UTF8));
    cookies->insert(std::pair<std::string, std::string>(cookie_name, cookie_value));
  }
}

int DocumentHost::AddCookie(const std::string& cookie) {
  CComBSTR cookie_bstr(CA2W(cookie.c_str(), CP_UTF8));

  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);

  if (!doc) {
    return EUNHANDLEDERROR;
  }

  if (!this->IsHtmlPage(doc)) {
    return ENOSUCHDOCUMENT;
  }

  if (FAILED(doc->put_cookie(cookie_bstr))) {
    return EUNHANDLEDERROR;
  }

  return SUCCESS;
}

int DocumentHost::DeleteCookie(const std::string& cookie_name) {
  // Construct the delete cookie script
  std::wstring script_source;
  for (int i = 0; DELETECOOKIES[i]; i++) {
    script_source += DELETECOOKIES[i];
  }

  CComPtr<IHTMLDocument2> doc;
  this->GetDocument(&doc);
  Script script_wrapper(doc, script_source, 1);
  script_wrapper.AddArgument(cookie_name);
  int status_code = script_wrapper.Execute();
  return status_code;
}

void DocumentHost::PostQuitMessage() {
  this->set_is_closing(true);
  LPSTR message_payload = new CHAR[this->browser_id_.size() + 1];
  strcpy_s(message_payload, this->browser_id_.size() + 1, this->browser_id_.c_str());
  ::PostMessage(this->executor_handle(),
                WD_BROWSER_QUIT,
                NULL,
                reinterpret_cast<LPARAM>(message_payload));
}

bool DocumentHost::IsHtmlPage(IHTMLDocument2* doc) {
  CComBSTR type;
  if (!SUCCEEDED(doc->get_mimeType(&type))) {
    return false;
  }

  std::wstring document_type_key_name= L"";
  if (this->factory_.GetRegistryValue(HKEY_CURRENT_USER,
                                      L"Software\\Microsoft\\Windows\\Shell\\Associations\\UrlAssociations\\http\\UserChoice",
                                      L"Progid",
                                      &document_type_key_name)) {
    // Look for the user-customization under Vista/Windows 7 first. If it's
    // IE, set the document friendly name lookup key to 'htmlfile'. If not,
    // set it to blank so that we can look up the proper HTML type.
    if (document_type_key_name == L"IE.HTTP") {
      document_type_key_name = L"htmlfile";
    } else {
      document_type_key_name = L"";
    }
  }

  if (document_type_key_name == L"") {
    // To be technically correct, we should look up the extension specified
    // for the text/html MIME type first (located in the "Extension" value
    // of HKEY_CLASSES_ROOT\MIME\Database\Content Type\text/html), but that
    // should always resolve to ".htm" anyway. From the extension, we can
    // find the browser-specific subkey of HKEY_CLASSES_ROOT, the default
    // value of which should contain the browser-specific friendly name of
    // the MIME type for HTML documents, which is what
    // IHTMLDocument2::get_mimeType() returns.
    if (!this->factory_.GetRegistryValue(HKEY_CLASSES_ROOT,
                                         L".htm",
                                         L"",
                                         &document_type_key_name)) {
      return false;
    }
  }

  std::wstring mime_type_name;
  if (!this->factory_.GetRegistryValue(HKEY_CLASSES_ROOT,
                                       document_type_key_name,
                                       L"",
                                       &mime_type_name)) {
    return false;
  }

  std::wstring type_string = type;
  return type_string == mime_type_name;
}

} // namespace webdriver<|endoftext|>


<commit_before>//
// Aspia Project
// Copyright (C) 2020 Dmitry Chapyshev <dmitry@aspia.ru>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program 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 this program. If not, see <https://www.gnu.org/licenses/>.
//

#include "base/crypto/big_num.h"

#include <gtest/gtest.h>

namespace base {

TEST(BigNumTest, Conversions)
{
    std::string hex = "00DF8CB233BD5EDF263CA842B91BC2B61AAEB92313B41CCBDEEB659EDDEAA53591D47EC559"
                      "E44F3B3A1202FFEA56EDCB11BD5D37824ACBB71E4316F3D5F63955";
    base::ByteArray salt = base::fromHex(hex);

    EXPECT_EQ(salt.size(), 64);

    base::BigNum salt_bn = base::BigNum::fromByteArray(salt);
    std::string salt_str = salt_bn.toStdString();

    EXPECT_EQ(salt_str.size(), 64);
}

} // namespace base
<commit_msg>Conversion result may be smaller than original size (as a result of cutting off non-significant zeros).<commit_after>//
// Aspia Project
// Copyright (C) 2020 Dmitry Chapyshev <dmitry@aspia.ru>
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program 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 this program. If not, see <https://www.gnu.org/licenses/>.
//

#include "base/crypto/big_num.h"

#include <gtest/gtest.h>

namespace base {

TEST(BigNumTest, Conversions)
{
    std::string hex = "00DF8CB233BD5EDF263CA842B91BC2B61AAEB92313B41CCBDEEB659EDDEAA53591D47EC559"
                      "E44F3B3A1202FFEA56EDCB11BD5D37824ACBB71E4316F3D5F63955";
    base::ByteArray salt = base::fromHex(hex);

    EXPECT_EQ(salt.size(), 64);

    base::BigNum salt_bn = base::BigNum::fromByteArray(salt);
    std::string salt_str = salt_bn.toStdString();

    EXPECT_EQ(salt_str.size(), 63);
}

} // namespace base
<|endoftext|>


<commit_before>#include "init.hpp"

#include "CatchingRule"
#include "PredicateRule"

using namespace sprout;

struct FakeException {};

BOOST_AUTO_TEST_CASE(testCatching)
{
    auto rule = makeCatching<FakeException>(
        makeAddingPredicate<char, std::string>([](const char& input) {
            if (input == 'a') {
                throw FakeException();
            }
            return true;
        })
    );

    {
        std::stringstream str("abc");
        auto cursor = makeCursor<char>(str);

        auto tokens = rule.parse(cursor);
        BOOST_CHECK(!tokens);
    }

    {
        std::stringstream str("bbb");
        auto cursor = makeCursor<char>(str);

        auto tokens = rule.parse(cursor);
        BOOST_REQUIRE(tokens);
        std::cout << *tokens << std::endl;
        BOOST_CHECK(*tokens == "bbb");
    }
}
<commit_msg>minor debug<commit_after>#include "init.hpp"

#include "CatchingRule"
#include "PredicateRule"

using namespace sprout;

struct FakeException {};

BOOST_AUTO_TEST_CASE(testCatching)
{
    auto rule = makeCatching<FakeException>(
        makeAddingPredicate<char, std::string>([](const char& input) {
            if (input == 'a') {
                throw FakeException();
            }
            return true;
        })
    );

    {
        std::stringstream str("abc");
        auto cursor = makeCursor<char>(str);

        auto tokens = rule.parse(cursor);
        BOOST_CHECK(!tokens);
    }

    {
        std::stringstream str("bbb");
        auto cursor = makeCursor<char>(str);

        auto tokens = rule.parse(cursor);
        BOOST_REQUIRE(tokens);
        BOOST_CHECK(*tokens == "bbb");
    }
}
<|endoftext|>


<commit_before>#include <ros/ros.h>

//ROS libraries
#include <tf/transform_datatypes.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/UInt8.h>

//Package include
#include <usbSerial.h>

using namespace std;

//aBridge functions
void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);
std::string getHumanFriendlyTime();

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
sensor_msgs::Imu imu;
nav_msgs::Odometry odom;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
const float deltaTime = 0.1; //abridge's update interval
int currentMode = 0;
string publishedName;

float heartbeat_publish_interval = 2;

//PID constants and arrays
const int histArrayLength = 1000;

float velFF = 0; //velocity feed forward
int stepV = 0; //keeps track of the point in the evArray for adding new error each update cycle.
float evArray[histArrayLength]; //history array of previous error for (arraySize/hz) seconds (error Velocity Array)
float velError[4] = {0,0,0,0}; //contains current velocity error and error 3 steps in the past.

int stepY = 0; //keeps track of the point in the eyArray for adding new error each update cycle.
float eyArray[histArrayLength]; //history array of previous error for (arraySize/hz) seconds (error Yaw Array)
float yawError[4] = {0,0,0,0}; //contains current yaw error and error 3 steps in the past.

float prevLin = 0;
float prevYaw = 0;

ros::Time prevDriveCommandUpdateTime;

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;
ros::Publisher infoLogPublisher;
ros::Publisher heartbeatPublisher;

//Subscribers
ros::Subscriber driveControlSubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;
ros::Subscriber modeSubscriber;

//Timers
ros::Timer publishTimer;
ros::Timer publish_heartbeat_timer;

//Callback handlers
void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event);
void modeHandler(const std_msgs::UInt8::ConstPtr& message);

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);
    ros::init(argc, argv, (hostname + "_ABRIDGE"));

    ros::NodeHandle param("~");
    string devicePath;
    param.param("device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);

    sleep(5);

    ros::NodeHandle aNH;

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  ABridge module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/fingerAngle/prev_cmd"), 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/wristAngle/prev_cmd"), 10);
    imuPublish = aNH.advertise<sensor_msgs::Imu>((publishedName + "/imu"), 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>((publishedName + "/odom"), 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarLeft"), 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarCenter"), 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarRight"), 10);
    infoLogPublisher = aNH.advertise<std_msgs::String>("/infoLog", 1, true);
    heartbeatPublisher = aNH.advertise<std_msgs::String>((publishedName + "/abridge/heartbeat"), 1, true);

    driveControlSubscriber = aNH.subscribe((publishedName + "/driveControl"), 10, driveCommandHandler);
    fingerAngleSubscriber = aNH.subscribe((publishedName + "/fingerAngle/cmd"), 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe((publishedName + "/wristAngle/cmd"), 1, wristAngleHandler);
    modeSubscriber = aNH.subscribe((publishedName + "/mode"), 1, modeHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);
    publish_heartbeat_timer = aNH.createTimer(ros::Duration(heartbeat_publish_interval), publishHeartBeatTimerEventHandler);

    imu.header.frame_id = publishedName+"/base_link";

    odom.header.frame_id = publishedName+"/odom";
    odom.child_frame_id = publishedName+"/base_link";

    for (int i = 0; i < histArrayLength; i++)
    {
    evArray[i] = 0;
    eyArray[i] = 0;
    }

    prevDriveCommandUpdateTime = ros::Time::now();

    ros::spin();

    return EXIT_SUCCESS;
}

//This command handler recives a linear velocity setpoint and a angular yaw error
//and produces a command output for the left and right motors of the robot.
//See the following paper for description of PID controllers.
//Bennett, Stuart (November 1984). "Nicholas Minorsky and the automatic steering of ships". IEEE Control Systems Magazine. 4 (4): 10–15. doi:10.1109/MCS.1984.1104827. ISSN 0272-1708.
void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {

  float left = (message->linear.x); //target linear velocity in meters per second
  float right = (message->angular.z); //angular error in radians

  // Cap motor commands at 120. Experimentally determined that high values (tested 180 and 255) can cause
  // the hardware to fail when the robot moves itself too violently.
  int max_motor_cmd = 120;

  if (currentMode == 1)
  {
    // Assumes left and right are always between -1 and 1
    float linear = left * max_motor_cmd;
    float angular = right * max_motor_cmd;

    left = linear - angular;
    right = linear + angular;
  }

  // Check that the resulting motor commands do not exceed the specified safe maximum value
  if (left > max_motor_cmd)
  {
    left = max_motor_cmd;
  }
  else if (left < -max_motor_cmd)
  {
    left = - max_motor_cmd;
  }

  if (right > max_motor_cmd)
  {
    right = max_motor_cmd;
  }
  else if (right < -max_motor_cmd)
  {
    right = -max_motor_cmd;
  }

  int leftInt = left;
  int rightInt = right;

  sprintf(moveCmd, "v,%d,%d\n", leftInt, rightInt); //format data for arduino into c string
  usb.sendData(moveCmd);                      //send movement command to arduino over usb
  memset(&moveCmd, '\0', sizeof (moveCmd));   //clear the movement command string
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void serialActivityTimer(const ros::TimerEvent& e) {
    usb.sendData(dataCmd);
    parseData(usb.readData());
    publishRosTopics();
}

void publishRosTopics() {
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuPublish.publish(imu);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
}

void parseData(string str) {
    istringstream oss(str);
    string sentence;

    while (getline(oss, sentence, '\n')) {
		istringstream wss(sentence);
		string word;

		vector<string> dataSet;
		while (getline(wss, word, ',')) {
			dataSet.push_back(word);
		}

		if (dataSet.size() >= 3 && dataSet.at(1) == "1") {

            if (dataSet.at(0) == "GRF") {
                fingerAngle.header.stamp = ros::Time::now();
                fingerAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
            }
			else if (dataSet.at(0) == "GRW") {
				wristAngle.header.stamp = ros::Time::now();
				wristAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "IMU") {
				imu.header.stamp = ros::Time::now();
				imu.linear_acceleration.x = atof(dataSet.at(2).c_str());
				imu.linear_acceleration.y = 0; //atof(dataSet.at(3).c_str());
				imu.linear_acceleration.z = atof(dataSet.at(4).c_str());
				imu.angular_velocity.x = atof(dataSet.at(5).c_str());
				imu.angular_velocity.y = atof(dataSet.at(6).c_str());
				imu.angular_velocity.z = atof(dataSet.at(7).c_str());
				imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(8).c_str()), atof(dataSet.at(9).c_str()), atof(dataSet.at(10).c_str()));
			}
			else if (dataSet.at(0) == "ODOM") {
				odom.header.stamp = ros::Time::now();
				odom.pose.pose.position.x += atof(dataSet.at(2).c_str()) / 100.0;
				odom.pose.pose.position.y += atof(dataSet.at(3).c_str()) / 100.0;
				odom.pose.pose.position.z = 0.0;
				odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(atof(dataSet.at(4).c_str()));
				odom.twist.twist.linear.x = atof(dataSet.at(5).c_str()) / 100.0;
				odom.twist.twist.linear.y = atof(dataSet.at(6).c_str()) / 100.0;
				odom.twist.twist.angular.z = atof(dataSet.at(7).c_str());
			}
			else if (dataSet.at(0) == "USL") {
				sonarLeft.header.stamp = ros::Time::now();
				sonarLeft.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USC") {
				sonarCenter.header.stamp = ros::Time::now();
				sonarCenter.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USR") {
				sonarRight.header.stamp = ros::Time::now();
				sonarRight.range = atof(dataSet.at(2).c_str()) / 100.0;
			}

		}
	}
}

void modeHandler(const std_msgs::UInt8::ConstPtr& message) {
	currentMode = message->data;
}

void publishHeartBeatTimerEventHandler(const ros::TimerEvent&) {
    std_msgs::String msg;
    msg.data = "";
    heartbeatPublisher.publish(msg);
}
<commit_msg>Fixed manual driving controle for physical rovers<commit_after>#include <ros/ros.h>

//ROS libraries
#include <tf/transform_datatypes.h>

//ROS messages
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <std_msgs/UInt8.h>

//Package include
#include <usbSerial.h>

using namespace std;

//aBridge functions
void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message);
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle);
void serialActivityTimer(const ros::TimerEvent& e);
void publishRosTopics();
void parseData(string data);
std::string getHumanFriendlyTime();

//Globals
geometry_msgs::QuaternionStamped fingerAngle;
geometry_msgs::QuaternionStamped wristAngle;
sensor_msgs::Imu imu;
nav_msgs::Odometry odom;
sensor_msgs::Range sonarLeft;
sensor_msgs::Range sonarCenter;
sensor_msgs::Range sonarRight;
USBSerial usb;
const int baud = 115200;
char dataCmd[] = "d\n";
char moveCmd[16];
char host[128];
const float deltaTime = 0.1; //abridge's update interval
int currentMode = 0;
string publishedName;

float heartbeat_publish_interval = 2;

//PID constants and arrays
const int histArrayLength = 1000;

float velFF = 0; //velocity feed forward
int stepV = 0; //keeps track of the point in the evArray for adding new error each update cycle.
float evArray[histArrayLength]; //history array of previous error for (arraySize/hz) seconds (error Velocity Array)
float velError[4] = {0,0,0,0}; //contains current velocity error and error 3 steps in the past.

int stepY = 0; //keeps track of the point in the eyArray for adding new error each update cycle.
float eyArray[histArrayLength]; //history array of previous error for (arraySize/hz) seconds (error Yaw Array)
float yawError[4] = {0,0,0,0}; //contains current yaw error and error 3 steps in the past.

float prevLin = 0;
float prevYaw = 0;

ros::Time prevDriveCommandUpdateTime;

//Publishers
ros::Publisher fingerAnglePublish;
ros::Publisher wristAnglePublish;
ros::Publisher imuPublish;
ros::Publisher odomPublish;
ros::Publisher sonarLeftPublish;
ros::Publisher sonarCenterPublish;
ros::Publisher sonarRightPublish;
ros::Publisher infoLogPublisher;
ros::Publisher heartbeatPublisher;

//Subscribers
ros::Subscriber driveControlSubscriber;
ros::Subscriber fingerAngleSubscriber;
ros::Subscriber wristAngleSubscriber;
ros::Subscriber modeSubscriber;

//Timers
ros::Timer publishTimer;
ros::Timer publish_heartbeat_timer;

//Callback handlers
void publishHeartBeatTimerEventHandler(const ros::TimerEvent& event);
void modeHandler(const std_msgs::UInt8::ConstPtr& message);

int main(int argc, char **argv) {

    gethostname(host, sizeof (host));
    string hostname(host);
    ros::init(argc, argv, (hostname + "_ABRIDGE"));

    ros::NodeHandle param("~");
    string devicePath;
    param.param("device", devicePath, string("/dev/ttyUSB0"));
    usb.openUSBPort(devicePath, baud);

    sleep(5);

    ros::NodeHandle aNH;

    if (argc >= 2) {
        publishedName = argv[1];
        cout << "Welcome to the world of tomorrow " << publishedName << "!  ABridge module started." << endl;
    } else {
        publishedName = hostname;
        cout << "No Name Selected. Default is: " << publishedName << endl;
    }

    fingerAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/fingerAngle/prev_cmd"), 10);
    wristAnglePublish = aNH.advertise<geometry_msgs::QuaternionStamped>((publishedName + "/wristAngle/prev_cmd"), 10);
    imuPublish = aNH.advertise<sensor_msgs::Imu>((publishedName + "/imu"), 10);
    odomPublish = aNH.advertise<nav_msgs::Odometry>((publishedName + "/odom"), 10);
    sonarLeftPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarLeft"), 10);
    sonarCenterPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarCenter"), 10);
    sonarRightPublish = aNH.advertise<sensor_msgs::Range>((publishedName + "/sonarRight"), 10);
    infoLogPublisher = aNH.advertise<std_msgs::String>("/infoLog", 1, true);
    heartbeatPublisher = aNH.advertise<std_msgs::String>((publishedName + "/abridge/heartbeat"), 1, true);

    driveControlSubscriber = aNH.subscribe((publishedName + "/driveControl"), 10, driveCommandHandler);
    fingerAngleSubscriber = aNH.subscribe((publishedName + "/fingerAngle/cmd"), 1, fingerAngleHandler);
    wristAngleSubscriber = aNH.subscribe((publishedName + "/wristAngle/cmd"), 1, wristAngleHandler);
    modeSubscriber = aNH.subscribe((publishedName + "/mode"), 1, modeHandler);

    publishTimer = aNH.createTimer(ros::Duration(deltaTime), serialActivityTimer);
    publish_heartbeat_timer = aNH.createTimer(ros::Duration(heartbeat_publish_interval), publishHeartBeatTimerEventHandler);

    imu.header.frame_id = publishedName+"/base_link";

    odom.header.frame_id = publishedName+"/odom";
    odom.child_frame_id = publishedName+"/base_link";

    for (int i = 0; i < histArrayLength; i++)
    {
    evArray[i] = 0;
    eyArray[i] = 0;
    }

    prevDriveCommandUpdateTime = ros::Time::now();

    ros::spin();

    return EXIT_SUCCESS;
}

//This command handler recives a linear velocity setpoint and a angular yaw error
//and produces a command output for the left and right motors of the robot.
//See the following paper for description of PID controllers.
//Bennett, Stuart (November 1984). "Nicholas Minorsky and the automatic steering of ships". IEEE Control Systems Magazine. 4 (4): 10–15. doi:10.1109/MCS.1984.1104827. ISSN 0272-1708.
void driveCommandHandler(const geometry_msgs::Twist::ConstPtr& message) {

  float left = (message->linear.x); //target linear velocity in meters per second
  float right = (message->angular.z); //angular error in radians

  // Cap motor commands at 120. Experimentally determined that high values (tested 180 and 255) can cause
  // the hardware to fail when the robot moves itself too violently.
  int max_motor_cmd = 120;

  // Check that the resulting motor commands do not exceed the specified safe maximum value
  if (left > max_motor_cmd)
  {
    left = max_motor_cmd;
  }
  else if (left < -max_motor_cmd)
  {
    left = - max_motor_cmd;
  }

  if (right > max_motor_cmd)
  {
    right = max_motor_cmd;
  }
  else if (right < -max_motor_cmd)
  {
    right = -max_motor_cmd;
  }

  int leftInt = left;
  int rightInt = right;

  sprintf(moveCmd, "v,%d,%d\n", leftInt, rightInt); //format data for arduino into c string
  usb.sendData(moveCmd);                      //send movement command to arduino over usb
  memset(&moveCmd, '\0', sizeof (moveCmd));   //clear the movement command string
}

// The finger and wrist handlers receive gripper angle commands in floating point
// radians, write them to a string and send that to the arduino
// for processing.
void fingerAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
  char cmd[16]={'\0'};

  // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'f' indicates this is a finger command to the arduino
    sprintf(cmd, "f,0\n");
  } else {
    sprintf(cmd, "f,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void wristAngleHandler(const std_msgs::Float32::ConstPtr& angle) {
    char cmd[16]={'\0'};

    // Avoid dealing with negative exponents which confuse the conversion to string by checking if the angle is small
  if (angle->data < 0.01) {
    // 'w' indicates this is a wrist command to the arduino
    sprintf(cmd, "w,0\n");
  } else {
    sprintf(cmd, "w,%.4g\n", angle->data);
  }
  usb.sendData(cmd);
  memset(&cmd, '\0', sizeof (cmd));
}

void serialActivityTimer(const ros::TimerEvent& e) {
    usb.sendData(dataCmd);
    parseData(usb.readData());
    publishRosTopics();
}

void publishRosTopics() {
    fingerAnglePublish.publish(fingerAngle);
    wristAnglePublish.publish(wristAngle);
    imuPublish.publish(imu);
    odomPublish.publish(odom);
    sonarLeftPublish.publish(sonarLeft);
    sonarCenterPublish.publish(sonarCenter);
    sonarRightPublish.publish(sonarRight);
}

void parseData(string str) {
    istringstream oss(str);
    string sentence;

    while (getline(oss, sentence, '\n')) {
		istringstream wss(sentence);
		string word;

		vector<string> dataSet;
		while (getline(wss, word, ',')) {
			dataSet.push_back(word);
		}

		if (dataSet.size() >= 3 && dataSet.at(1) == "1") {

            if (dataSet.at(0) == "GRF") {
                fingerAngle.header.stamp = ros::Time::now();
                fingerAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
            }
			else if (dataSet.at(0) == "GRW") {
				wristAngle.header.stamp = ros::Time::now();
				wristAngle.quaternion = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(2).c_str()), 0.0, 0.0);
			}
			else if (dataSet.at(0) == "IMU") {
				imu.header.stamp = ros::Time::now();
				imu.linear_acceleration.x = atof(dataSet.at(2).c_str());
				imu.linear_acceleration.y = 0; //atof(dataSet.at(3).c_str());
				imu.linear_acceleration.z = atof(dataSet.at(4).c_str());
				imu.angular_velocity.x = atof(dataSet.at(5).c_str());
				imu.angular_velocity.y = atof(dataSet.at(6).c_str());
				imu.angular_velocity.z = atof(dataSet.at(7).c_str());
				imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(dataSet.at(8).c_str()), atof(dataSet.at(9).c_str()), atof(dataSet.at(10).c_str()));
			}
			else if (dataSet.at(0) == "ODOM") {
				odom.header.stamp = ros::Time::now();
				odom.pose.pose.position.x += atof(dataSet.at(2).c_str()) / 100.0;
				odom.pose.pose.position.y += atof(dataSet.at(3).c_str()) / 100.0;
				odom.pose.pose.position.z = 0.0;
				odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(atof(dataSet.at(4).c_str()));
				odom.twist.twist.linear.x = atof(dataSet.at(5).c_str()) / 100.0;
				odom.twist.twist.linear.y = atof(dataSet.at(6).c_str()) / 100.0;
				odom.twist.twist.angular.z = atof(dataSet.at(7).c_str());
			}
			else if (dataSet.at(0) == "USL") {
				sonarLeft.header.stamp = ros::Time::now();
				sonarLeft.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USC") {
				sonarCenter.header.stamp = ros::Time::now();
				sonarCenter.range = atof(dataSet.at(2).c_str()) / 100.0;
			}
			else if (dataSet.at(0) == "USR") {
				sonarRight.header.stamp = ros::Time::now();
				sonarRight.range = atof(dataSet.at(2).c_str()) / 100.0;
			}

		}
	}
}

void modeHandler(const std_msgs::UInt8::ConstPtr& message) {
	currentMode = message->data;
}

void publishHeartBeatTimerEventHandler(const ros::TimerEvent&) {
    std_msgs::String msg;
    msg.data = "";
    heartbeatPublisher.publish(msg);
}
<|endoftext|>


<commit_before>/* algorithm.cpp - CS 472 Project #2: Genetic Programming
 * Copyright 2014 Andrew Schwartzmeyer
 *
 * Source file for algorithm namespace
 */

#include <algorithm>
#include <cassert>
#include <ctime>
#include <iostream>
#include <fstream>
#include <future>
#include <thread>

#include "algorithm.hpp"
#include "../individual/individual.hpp"
#include "../options/options.hpp"
#include "../random_generator/random_generator.hpp"

namespace algorithm
{
  using std::vector;
  using individual::Individual;
  using options::Options;
  using namespace random_generator;

  // Returns true if "a" is closer to 0 than "b" and is also normal.
  bool
  compare_fitness::operator()(const Individual& a, const Individual& b)
  {
    return std::isnormal(a.get_fitness())
      ? (a.get_fitness() > b.get_fitness()) : false;
  }

  // Opens the appropriate log file for given time, trial, and folder.
  void
  open_log(std::ofstream& log, const std::time_t& time, const int& trial,
	   const std::string& folder)
  {
    std::string filename = folder + std::to_string(time) + "_"
      + std::to_string(trial) + ".dat";

    log.open(filename, std::ios_base::app);
    if (not log.is_open())
      {
	std::cerr << "Log file " << filename << " could not be opened!\n";
	std::exit(EXIT_FAILURE);
      }
  }

  /* Log a line of relevant algorithm information (best and average
     fitness and size plus adjusted best fitness). */
  void
  log_info(const unsigned int verbosity, const std::string& logs_dir,
	   const std::time_t& time, const int& trial, const int& iteration,
	   const Individual& best, const vector <Individual>& population)
  {
    // Don't log if verbosity is zero, but still allow calls to this function.
    if (verbosity == 0)
      return;

    float total_fitness =
      std::accumulate(population.begin(), population.end(), 0.,
		      [](const float& a, const Individual& b)->float const
		      {
			return a + b.get_adjusted();
		      });

    int total_size =
      std::accumulate(population.begin(), population.end(), 0,
		      [](const unsigned int& a, const Individual& b)->unsigned int const
		      {
			return a + b.get_total();
		      });

    std::ofstream log;
    open_log(log, time, trial, logs_dir);
    log << iteration << "\t"
	<< best.get_score() << "\t"
	<< best.get_adjusted() << "\t"
	<< total_fitness / population.size() << "\t"
	<< best.get_total() << "\t"
	<< total_size / population.size() << "\n";
    log.close();
  }

  /* Create an initial population using "ramped half-and-half" (half
     full trees, half randomly grown trees, all to random depths
     between 0 and maximum depth). */
  vector<Individual>
  new_population(const Options& options)
  {
    vector<Individual> population;
    int_dist depth_dist{0, static_cast<int>(options.max_depth)}; // ramped
    for (unsigned int i = 0; i < options.population_size; ++i)
      {
	/* The depth is ramped, and so drawn randomly for each
	   individual. */
	unsigned int depth = depth_dist(rg.engine);
	population.emplace_back(Individual{
	    depth, options.grow_chance, options.map});
      }
    return population;
  }

  /* Return best candidate from size number of contestants randomly
     drawn from population. */
  Individual
  selection(const unsigned int& size, const vector<Individual>& population)
  {
    int_dist dist{0, static_cast<int>(population.size()) - 1}; // closed interval
    vector<Individual> contestants;

    for (unsigned int i = 0; i < size; ++i)
      contestants.emplace_back(population[dist(rg.engine)]);

    return *std::min_element(contestants.begin(), contestants.end(),
			     compare_fitness());
  }

  /* Return new offspring population reassembled from parallel calls to
     get_children () delegate. */
  vector<Individual>
  new_offspring(const Options& options, const vector<Individual>& population)
  {
    vector<Individual> offspring;
    offspring.reserve(population.size());
    // Select parents for children.
    while (offspring.size() != population.size())
      {
	offspring.emplace_back(selection(options.tournament_size, population));
      }
    // Binary crossover with probability.
    if (options.crossover_size == 2)
      {
	real_dist dist{0, 1};
	for (unsigned int i = 0; i < offspring.size(); i += 2)
	  {
	    if (dist(rg.engine) < options.crossover_chance)
	      {
		crossover(options.internals_chance,
			  offspring[i], offspring[i + 1]);
	      }
	  }
      }
    // Mutate and evaluate children.
    for (Individual& child : offspring)
      {
	child.mutate(options.mutate_chance); // Mutate children
	child.evaluate(options.map, options.penalty); // Evaluate children
      }
    assert(offspring.size() == population.size());
    return offspring;
  }

  /* The actual genetic algorithm applied which (hopefully) produces a
     well-fit expression for a given dataset. */
  const individual::Individual
  genetic(const Options& options, const std::time_t time, const int& trial)
  {
    // Start logging
    std::ofstream log;
    if (options.verbosity > 0)
      {
	open_log(log, time, trial, options.logs_dir);
	log << "# running a Genetic Program @ "
	    << std::ctime(&time)
	    << "# initial depth: " << options.max_depth
	    << ", iterations: " << options.iterations
	    << ", population size: " << options.population_size
	    << ", tournament size: " << options.tournament_size << "\n"
	    << "# raw fitness - best (adjusted) fitness - average (adjusted) fitness - size of best - average size\n";
	log.close();
      }
    // Begin timing algorithm.
    std::chrono::time_point<std::chrono::system_clock> start, end;
    start = std::chrono::system_clock::now();

    // Create initial population.
    vector<Individual> population = new_population(options);
    Individual best;

    // Run algorithm to termination.
    for (unsigned int i = 0; i < options.iterations; ++i)
      {
	// Find best Individual of current population.
	best = *std::min_element(population.begin(), population.end(),
				 compare_fitness());
	// Launch background logging thread.
	auto log_thread = std::async(std::launch::async, log_info,
				     options.verbosity, options.logs_dir, time,
				     trial, i, best, population);
	// Create replacement population.
	vector<Individual> offspring = new_offspring(options, population);
	// Perform elitism replacement of random individuals.
	int_dist dist{0, static_cast<int>(options.population_size) - 1};
	for (unsigned int e = 0; e < options.elitism_size; ++e)
	  offspring[dist(rg.engine)] = best;
	// Replace current population with offspring.
	population = offspring;
	// Sync with background logging thread.
	log_thread.wait();
      }
    // End timing algorithm.
    end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    std::time_t end_time = std::chrono::system_clock::to_time_t(end);

    // Log time information.
    if (options.verbosity > 0)
      {
	open_log(log, time, trial, options.logs_dir);
	log << best.print() << best.print_formula()
	    << "# finished computation @ " << std::ctime(&end_time)
	    << "# elapsed time: " << elapsed_seconds.count() << "s\n";
	log.close();
      }
    // Log evaluation plot data of best individual.
    std::ofstream plot;
    open_log(plot, time, trial, options.plots_dir);
    plot << best.evaluate(options.map, options.penalty, true);
    plot.close();
    return best;
  }
}
<commit_msg>Updating log output<commit_after>/* algorithm.cpp - CS 472 Project #2: Genetic Programming
 * Copyright 2014 Andrew Schwartzmeyer
 *
 * Source file for algorithm namespace
 */

#include <algorithm>
#include <cassert>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <future>
#include <thread>

#include "algorithm.hpp"
#include "../individual/individual.hpp"
#include "../options/options.hpp"
#include "../random_generator/random_generator.hpp"

namespace algorithm
{
  using std::vector;
  using individual::Individual;
  using options::Options;
  using namespace random_generator;

  // Returns true if "a" is closer to 0 than "b" and is also normal.
  bool
  compare_fitness::operator()(const Individual& a, const Individual& b)
  {
    return std::isnormal(a.get_fitness())
      ? (a.get_fitness() > b.get_fitness()) : false;
  }

  // Opens the appropriate log file for given time, trial, and folder.
  void
  open_log(std::ofstream& log, const std::time_t& time, const int& trial,
	   const std::string& folder)
  {
    std::string filename = folder + std::to_string(time) + "_"
      + std::to_string(trial) + ".dat";

    log.open(filename, std::ios_base::app);
    if (not log.is_open())
      {
	std::cerr << "Log file " << filename << " could not be opened!\n";
	std::exit(EXIT_FAILURE);
      }
  }

  /* Log a line of relevant algorithm information (best and average
     fitness and size plus adjusted best fitness). */
  void
  log_info(const unsigned int verbosity, const std::string& logs_dir,
	   const std::time_t& time, const int& trial, const int& iteration,
	   const Individual& best, const vector<Individual>& population)
  {
    // Don't log if verbosity is zero, but still allow calls to this function.
    if (verbosity == 0)
      return;

    float total_fitness =
      std::accumulate(population.begin(), population.end(), 0.,
		      [](const float& a, const Individual& b)->float const
		      {
			return a + b.get_adjusted();
		      });

    unsigned int total_size =
      std::accumulate(population.begin(), population.end(), 0,
		      [](const unsigned int& a, const Individual& b)->unsigned int const
		      {
			return a + b.get_total();
		      });

    unsigned int total_depth =
      std::accumulate(population.begin(), population.end(), 0,
		      [](const unsigned int& a, const Individual& b)->unsigned int const
		      {
			return a + b.get_depth();
		      });

    std::ofstream log;
    open_log(log, time, trial, logs_dir);
    log << std::setprecision(4)
	<< iteration << "\t"
	<< best.get_score() << "\t"
	<< best.get_adjusted() << "\t"
	<< total_fitness / population.size() << "\t"
	<< best.get_total() << "\t"
	<< total_size / population.size()
	<< best.get_depth() << "\t"
	<< total_depth / population.size() << "\n";
    log.close();
  }

  /* Create an initial population using "ramped half-and-half" (half
     full trees, half randomly grown trees, all to random depths
     between 0 and maximum depth). */
  vector<Individual>
  new_population(const Options& options)
  {
    vector<Individual> population;
    int_dist depth_dist{0, static_cast<int>(options.max_depth)}; // ramped
    for (unsigned int i = 0; i < options.population_size; ++i)
      {
	/* The depth is ramped, and so drawn randomly for each
	   individual. */
	unsigned int depth = depth_dist(rg.engine);
	population.emplace_back(Individual{
	    depth, options.grow_chance, options.map});
      }
    return population;
  }

  /* Return best candidate from size number of contestants randomly
     drawn from population. */
  Individual
  selection(const unsigned int& size, const vector<Individual>& population)
  {
    int_dist dist{0, static_cast<int>(population.size()) - 1}; // closed interval
    vector<Individual> contestants;

    for (unsigned int i = 0; i < size; ++i)
      contestants.emplace_back(population[dist(rg.engine)]);

    return *std::min_element(contestants.begin(), contestants.end(),
			     compare_fitness());
  }

  /* Return new offspring population reassembled from parallel calls to
     get_children () delegate. */
  vector<Individual>
  new_offspring(const Options& options, const vector<Individual>& population)
  {
    vector<Individual> offspring;
    offspring.reserve(population.size());
    // Select parents for children.
    while (offspring.size() != population.size())
      {
	offspring.emplace_back(selection(options.tournament_size, population));
      }
    // Binary crossover with probability.
    if (options.crossover_size == 2)
      {
	real_dist dist{0, 1};
	for (unsigned int i = 0; i < offspring.size(); i += 2)
	  {
	    if (dist(rg.engine) < options.crossover_chance)
	      {
		crossover(options.internals_chance,
			  offspring[i], offspring[i + 1]);
	      }
	  }
      }
    // Mutate and evaluate children.
    for (Individual& child : offspring)
      {
	child.mutate(options.mutate_chance); // Mutate children
	child.evaluate(options.map, options.penalty); // Evaluate children
      }
    assert(offspring.size() == population.size());
    return offspring;
  }

  /* The actual genetic algorithm applied which (hopefully) produces a
     well-fit expression for a given dataset. */
  const individual::Individual
  genetic(const Options& options, const std::time_t time, const int& trial)
  {
    // Start logging
    std::ofstream log;
    if (options.verbosity > 0)
      {
	open_log(log, time, trial, options.logs_dir);
	log << "# running a Genetic Program @ "
	    << std::ctime(&time)
	    << "# initial depth: " << options.max_depth
	    << ", iterations: " << options.iterations
	    << ", population size: " << options.population_size
	    << ", tournament size: " << options.tournament_size
	    << ", elitism size: " << options.elitism_size
	    << ", fitness penalty: " << options.penalty << " * total size"
	    << ", crossover chance: " << options.crossover_chance
	    << ", mutate chance: " << options.mutate_chance
	    << ", grow chance: " << options.grow_chance << "\n"
	    << "# score - best a. fitness - average a. fitness - best size - average size - best depth - average depth\n";
	log.close();
      }
    // Begin timing algorithm.
    std::chrono::time_point<std::chrono::system_clock> start, end;
    start = std::chrono::system_clock::now();

    // Create initial population.
    vector<Individual> population = new_population(options);
    Individual best;

    // Run algorithm to termination.
    for (unsigned int i = 0; i < options.iterations; ++i)
      {
	// Find best Individual of current population.
	best = *std::min_element(population.begin(), population.end(),
				 compare_fitness());
	// Launch background logging thread.
	auto log_thread = std::async(std::launch::async, log_info,
				     options.verbosity, options.logs_dir, time,
				     trial, i, best, population);
	// Create replacement population.
	vector<Individual> offspring = new_offspring(options, population);
	// Perform elitism replacement of random individuals.
	int_dist dist{0, static_cast<int>(options.population_size) - 1};
	for (unsigned int e = 0; e < options.elitism_size; ++e)
	  offspring[dist(rg.engine)] = best;
	// Replace current population with offspring.
	population = offspring;
	// Sync with background logging thread.
	log_thread.wait();
      }
    // End timing algorithm.
    end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    std::time_t end_time = std::chrono::system_clock::to_time_t(end);

    // Log time information.
    if (options.verbosity > 0)
      {
	open_log(log, time, trial, options.logs_dir);
	log << best.print() << best.print_formula()
	    << "# finished computation @ " << std::ctime(&end_time)
	    << "# elapsed time: " << elapsed_seconds.count() << "s\n";
	log.close();
      }
    // Log evaluation plot data of best individual.
    std::ofstream plot;
    open_log(plot, time, trial, options.plots_dir);
    plot << best.evaluate(options.map, options.penalty, true);
    plot.close();
    return best;
  }
}
<|endoftext|>