text:
#include "ros/ros.h"
#include "autoware_msgs/image_obj.h"
#include "autoware_msgs/PointsImage.h"
#include "autoware_msgs/image_obj_ranged.h"
#include "sync.hpp"

int main(int argc, char **argv) {
    ros::init(argc, argv, "sync_ranging");
    std::string ns(ros::this_node::getNamespace());
    std::string sub1("/image_obj");
    std::string sub2("/vscan_image");
    std::string req("/image_obj_ranged");
    std::string pub1("/image_obj");
    std::string pub2("/vscan_image");

    Synchronizer<autoware_msgs::image_obj, autoware_msgs::PointsImage, autoware_msgs::image_obj_ranged> synchronizer(sub1, sub2, pub1, pub2, req, ns);
    synchronizer.run();

    return 0;
}

#if 0
/* ----header---- */
/* common header */
#include "ros/ros.h"
#include <ros/callback_queue.h>
#include <boost/circular_buffer.hpp>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sys/stat.h>
#include <sys/select.h>
#include <mqueue.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <pthread.h>
#include "t_sync_message.h"
/* user header */
#include "autoware_msgs/image_obj.h"
#include "autoware_msgs/PointsImage.h"
#include "autoware_msgs/image_obj_ranged.h"

/* ----mode---- */
#define _REQ_PUB 1

/* ----var---- */
/* common var */
bool buf_flag;
pthread_mutex_t mutex;
/* user var */
boost::circular_buffer<autoware_msgs::image_obj> image_obj_ringbuf(10);
boost::circular_buffer<autoware_msgs::PointsImage> vscan_image_ringbuf(10);
ros::Publisher image_obj__pub;
ros::Publisher vscan_image__pub;
bool image_obj_ranged_flag;

/* ----function---- */
double fabs_time_diff(std_msgs::Header *timespec1, std_msgs::Header *timespec2) {
    double time1 = (double)timespec1->stamp.sec + (double)timespec1->stamp.nsec/1000000000L;
    double time2 = (double)timespec2->stamp.sec + (double)timespec2->stamp.nsec/1000000000L;

    return fabs(time1 - time2);
}

double get_time(const std_msgs::Header *timespec) {
    return (double)timespec->stamp.sec + (double)timespec->stamp.nsec/1000000000L;
}

#if _REQ_PUB
autoware_msgs::image_obj* p_image_obj_buf;
autoware_msgs::PointsImage* p_vscan_image_buf;

void publish_msg(autoware_msgs::image_obj* p_image_obj_buf, autoware_msgs::PointsImage* p_vscan_image_buf) {
    ROS_INFO("publish");
    image_obj__pub.publish(*p_image_obj_buf);
    vscan_image__pub.publish(*p_vscan_image_buf);
}

bool publish() {
    if (buf_flag) {
        //image_obj is empty
        if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
            ROS_INFO("image_obj ring buffer is empty");
            return false;
        }

        //vscan_image is empty
        if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
            ROS_INFO("vscan_image ring buffer is empty");
            return false;
        }

        // image_obj > vscan_image
        if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
            p_vscan_image_buf = &(vscan_image_ringbuf.front());
            boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
            if (image_obj_ringbuf.size() == 1) {
                p_image_obj_buf = &*it;
                publish_msg(p_image_obj_buf, p_vscan_image_buf);
                if (image_obj_ranged_flag == true){
                    buf_flag = false;
                    image_obj_ranged_flag = false;
                    image_obj_ringbuf.clear();
                    vscan_image_ringbuf.clear();
                }
                return true;
            } else {
                for (it++; it != image_obj_ringbuf.end(); it++) {
                    if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
                        < fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
                        p_image_obj_buf = &*(it-1);
                        break;
                    }
                }
                if (it == image_obj_ringbuf.end()) {
                    p_image_obj_buf = &(image_obj_ringbuf.back());
                }
            }
        }
        // image_obj < vscan_image
        else {
            p_image_obj_buf = &(image_obj_ringbuf.front());
            boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
            if (vscan_image_ringbuf.size() == 1) {
                p_vscan_image_buf = &*it;
                publish_msg(p_image_obj_buf, p_vscan_image_buf);
                if (image_obj_ranged_flag == true){
                    buf_flag = false;
                    image_obj_ranged_flag = false;
                    image_obj_ringbuf.clear();
                    vscan_image_ringbuf.clear();
                }
                return true;
            }

            for (it++; it != vscan_image_ringbuf.end(); it++) {
                if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
                    p_vscan_image_buf = &*(it-1);
                    break;
                }
            }

            if (it == vscan_image_ringbuf.end()) {
                p_vscan_image_buf = &(vscan_image_ringbuf.back());
            }
        }
        publish_msg(p_image_obj_buf, p_vscan_image_buf);
        if (image_obj_ranged_flag == true){
            buf_flag = false;
            image_obj_ranged_flag = false;
            image_obj_ringbuf.clear();
            vscan_image_ringbuf.clear();
        }

        return true;
    } else {
        return false;
    }
}

void image_obj_callback(const autoware_msgs::image_obj::ConstPtr& image_obj_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ringbuf.push_front(*image_obj_msg);
    //vscan_image is empty
    if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
        ROS_INFO("vscan_image ring buffer is empty");
        buf_flag = false;
        pthread_mutex_unlock(&mutex);
        return;
    }
    buf_flag = true;
    pthread_mutex_unlock(&mutex);
    pthread_mutex_lock(&mutex);
    if (image_obj_ranged_flag == true) {
        publish();
    }
    pthread_mutex_unlock(&mutex);
}

void vscan_image_callback(const autoware_msgs::PointsImage::ConstPtr& vscan_image_msg) {
    pthread_mutex_lock(&mutex);
    vscan_image_ringbuf.push_front(*vscan_image_msg);
    //image_obj is empty
    if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
        ROS_INFO("image_obj ring buffer is empty");
        buf_flag = false;
        pthread_mutex_unlock(&mutex);
        return;
    }
    buf_flag = true;
    pthread_mutex_unlock(&mutex);
    pthread_mutex_lock(&mutex);
    if (image_obj_ranged_flag == true) {
        publish();
    }
    pthread_mutex_unlock(&mutex);
}
#else
#endif

void image_obj_ranged_callback(const autoware_msgs::image_obj_ranged::ConstPtr& image_obj_ranged_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ranged_flag = true;
    ROS_INFO("catch publish request");
    if (publish() == false) {
        ROS_INFO("waitting...");
    }
    pthread_mutex_unlock(&mutex);
}

void* thread(void* args) {
    ros::NodeHandle nh_rcv;
    ros::CallbackQueue rcv_callbackqueue;
    nh_rcv.setCallbackQueue(&rcv_callbackqueue);
    ros::Subscriber image_obj_ranged_sub = nh_rcv.subscribe("/image_obj_ranged", 1, image_obj_ranged_callback);
    while (nh_rcv.ok()) {
        rcv_callbackqueue.callAvailable(ros::WallDuration(3.0f));
        pthread_mutex_lock(&mutex);
        bool flag = (image_obj_ranged_flag == false && buf_flag == true);
        if (flag) {
            ROS_INFO("timeout");
            if(!publish()) {
                /* when to publish is failure, republish */
                struct timespec sleep_time;
                sleep_time.tv_sec = 0;
                sleep_time.tv_nsec = 200000000; //5Hz
                while (!publish() && ros::ok())
                    nanosleep(&sleep_time, NULL);
            }
        }
        pthread_mutex_unlock(&mutex);

    }
    return NULL;
}

int main(int argc, char **argv) {
    /* init */
    buf_flag = false;
    image_obj_ranged_flag = false;
    ros::init(argc, argv, "sync_ranging");
    ros::NodeHandle nh;

    /* create server thread */
    pthread_t th;
    pthread_create(&th, NULL, thread, (void *)NULL );

    ros::Subscriber image_obj_sub = nh.subscribe("/image_obj", 1, image_obj_callback);
    ros::Subscriber vscan_image_sub = nh.subscribe("/vscan_image", 1, vscan_image_callback);
    image_obj__pub = nh.advertise<autoware_msgs::image_obj>("/sync_ranging/image_obj", 5);
    vscan_image__pub = nh.advertise<autoware_msgs::PointsImage>("/sync_ranging/vscan_image", 5);

    while ((!buf_flag) && ros::ok()) {
        ros::spinOnce();
        usleep(100000);
    }
    pthread_mutex_lock(&mutex);
    if(!publish()) {
        /* when to publish is failure, republish */
        struct timespec sleep_time;
        sleep_time.tv_sec = 0;
        sleep_time.tv_nsec = 200000000; //5Hz
        while (!publish() && ros::ok())
            nanosleep(&sleep_time, NULL);
    }
    pthread_mutex_unlock(&mutex);

    ros::spin();
    pthread_mutex_unlock(&mutex);

    /* shutdown server thread */
    ROS_INFO("wait until shutdown a thread");
    pthread_kill(th, SIGINT);
    pthread_join(th, NULL);

    return 0;
}

#if 0
autoware_msgs::image_obj image_obj_buf;
autoware_msgs::PointsImage vscan_image_buf;

void image_obj_callback(const autoware_msgs::image_obj::ConstPtr& image_obj_msg) {
    pthread_mutex_lock(&mutex);
    image_obj_ringbuf.push_front(*image_obj_msg);

    //vscan_image is empty
    if (vscan_image_ringbuf.begin() == vscan_image_ringbuf.end()) {
        pthread_mutex_unlock(&mutex);
        ROS_INFO("vscan_image ring buffer is empty");
        return;
    }

    buf_flag = true;

    // image_obj > vscan_image
    if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
        vscan_image_buf = vscan_image_ringbuf.front();
        boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
        if (image_obj_ringbuf.size() == 1) {
            image_obj_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        } else {
            for (it++; it != image_obj_ringbuf.end(); it++) {
                if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
                    image_obj_buf = *(it-1);
                    break;
                }
            }
            if (it == image_obj_ringbuf.end()) {
                image_obj_buf = image_obj_ringbuf.back();
            }
        }

    } else {
        image_obj_buf = image_obj_ringbuf.front();
        boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
        if (vscan_image_ringbuf.size() == 1) {
            vscan_image_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        }

        for (it++; it != vscan_image_ringbuf.end(); it++) {
            if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
                < fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
                vscan_image_buf = *(it-1);
                break;
            }
        }

        if (it == vscan_image_ringbuf.end()) {
            vscan_image_buf = vscan_image_ringbuf.back();
        }
    }
    pthread_mutex_unlock(&mutex);
}

void vscan_image_callback(const autoware_msgs::PointsImage::ConstPtr& vscan_image_msg) {
    pthread_mutex_lock(&mutex);
    vscan_image_ringbuf.push_front(*vscan_image_msg);
    //image_obj is empty
    if (image_obj_ringbuf.begin() == image_obj_ringbuf.end()) {
        ROS_INFO("image_obj ring buffer is empty");
        pthread_mutex_unlock(&mutex);
        return;
    }

    buf_flag = true;

    // image_obj > vscan_image
    if (get_time(&(image_obj_ringbuf.front().header)) >= get_time(&(vscan_image_ringbuf.front().header))) {
        vscan_image_buf = vscan_image_ringbuf.front();
        boost::circular_buffer<autoware_msgs::image_obj>::iterator it = image_obj_ringbuf.begin();
        if (image_obj_ringbuf.size() == 1) {
            image_obj_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        } else {
            for (it++; it != image_obj_ringbuf.end(); it++) {
                if (fabs_time_diff(&(vscan_image_ringbuf.front().header), &((it-1)->header))
                    < fabs_time_diff(&(vscan_image_ringbuf.front().header), &(it->header))) {
                    image_obj_buf = *(it-1);
                    break;
                }
            }
            if (it == image_obj_ringbuf.end()) {
                image_obj_buf = image_obj_ringbuf.back();
            }
        }

    } else {
        image_obj_buf = image_obj_ringbuf.front();
        boost::circular_buffer<autoware_msgs::PointsImage>::iterator it = vscan_image_ringbuf.begin();
        if (vscan_image_ringbuf.size() == 1) {
            vscan_image_buf = *it;
            pthread_mutex_unlock(&mutex);
            return;
        }

        for (it++; it != vscan_image_ringbuf.end(); it++) {
            if (fabs_time_diff(&(image_obj_ringbuf.front().header), &((it-1)->header))
                < fabs_time_diff(&(image_obj_ringbuf.front().header), &(it->header))) {
                vscan_image_buf = *(it-1);
                break;
            }
        }

        if (it == vscan_image_ringbuf.end()) {
            vscan_image_buf = vscan_image_ringbuf.back();
        }
    }
    pthread_mutex_unlock(&mutex);
}

bool publish() {
    if (buf_flag) {
        pthread_mutex_lock(&mutex);
        // scan_ringbuf.clear();
        // image_ringbuf.clear();
        // scan_ringbuf.push_front(scan_buf);
        // image_ringbuf.push_front(image_buf);
        ROS_INFO("publish");
        image_obj__pub.publish(image_obj_buf);
        vscan_image__pub.publish(vscan_image_buf);
        pthread_mutex_unlock(&mutex);
        return true;
    } else {
        ROS_INFO("publish failed");
        return false;
    }
}

#endif
#endif

max_stars_repo_name: izeki/Autoware

max_issues_repo_name: izeki/Autoware

max_forks_repo_name: izeki/Autoware


text:
// Bota Pelota.cpp : Este archivo contiene la función "main". La ejecución del programa comienza y termina ahí.
//

#include <iostream>
#include <stdio.h>
#include <conio.h>
#include <stdlib.h>
#include <windows.h>
#include <time.h>
#include <gl/GLU.h>
#include  <GLFW/glfw3.h>
#include <armadillo>
#include "Lector.h"
#include "Objeto.h"
#include "Transform.h"
#include "Pokebola.h"
#include "Pokemon.h"

arma::frowvec posicionpikachu() {
	srand(time(NULL));
	float x = ((rand() % 40) / 10.0) - 2.0;
	float y = 0.0;
	float z = ((rand() % 50) / 10.0)*-1.0;
	arma::frowvec pos = { x,y,z };
	return pos;
}

int main(void)
{
	//valores del usuario
	int velocidad = 0;
	while (velocidad < 1 || velocidad>5) {
		cout << "Escoge velocidad (1-5):" << endl;
		cin >> velocidad;
	}
	int dx=-10;
	int dy=-10;
	int dz = 0;
	while (dx < 0) {
		cout << "Direcci�n en X :" << endl;
		cin >> dx;
	}
	while (dy < 0) {
		cout << "Direcci�n en Y :" << endl;
		cin >> dy;
	}
	cout << "Direcci�n en z :" << endl;
	cin >> dz;

	//camara y ojo
	arma::frowvec eye = { -10.0, 0.5, 0.0 };
	arma::frowvec camera = { 0.0, 0.0, 0.0 };
	arma::frowvec DOP = arma::normalise(camera - eye);
	float angley = 0.0;

	GLFWwindow* window;

	Lector lectorPokebola = Lector("pokeball3.obj");
	Lector lectorPokemon = Lector("pikachu.obj");

	lectorPokebola.read();
	lectorPokemon.read();

	Objeto pokebola_o = lectorPokebola.objetos[0];
	Objeto pokemon_o = lectorPokemon.objetos[0];

	//POKEBOLA
	std::vector< Face > pokebola_faces = pokebola_o.faces;
	Transform Tr = Transform();
	arma::fmat transf = Tr.S(0.1,0.1,0.1)*Tr.T(-3.0,0.0, 0.0);
	float radio = pokebola_o.biggestOne;
	//Transformacion, caras, colores, T, R, S, velocidad
	Pokebola pokebola = Pokebola(transf, pokebola_faces, { 1.0, 0.0, 0.0 }, velocidad, dx, dy, dz, radio);

	//POKEMON
	arma::frowvec posicion_pokemon = posicionpikachu();
	//posicion_pokemon = { -1,0,-1 };
	std::vector<Face> pokemon_faces = pokemon_o.faces;
	arma::fmat transf_pokemon = Tr.S(0.3,0.3,0.3)*Tr.R(0, 1, 0, -90.0)*Tr.T(posicion_pokemon[0], posicion_pokemon[1], posicion_pokemon[2]);
	//Transformacion, caras, colores, S, V
	radio = pokemon_o.biggestOne;
	Pokemon pokemon = Pokemon(transf_pokemon, pokemon_faces, { 1.0, 1.0, 0.0 }, 0.7, 0, posicion_pokemon, radio);
	pokemon.mueve(transf_pokemon);

	if (!glfwInit())
	{
		fprintf(stderr, "Fallo al inicializar GLFW\n");
		getchar();
		return -1;
	}

	window = glfwCreateWindow(1024, 768, "Pokemon Go VECG", NULL, NULL);
	if (window == NULL) {
		fprintf(stderr, "Fallo al abrir la ventana de GLFW.\n");
		getchar();
		glfwTerminate();
		return -1;
	}
	glfwMakeContextCurrent(window);
	glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LESS);

	glClearColor(0.7f, 0.7f, 0.7f, 0.0f);

	//  Proyecciones
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();

	int width, height;
	glfwGetFramebufferSize(window, &width, &height);

	float ar = width / height;

	//  Proyecci�n en paralelo
	glViewport(0, 0, width, height);
	glOrtho(-ar, ar, -1.0, 1.0, -40.0, 40.0);

	//  Proyecci�n en perspectiva
	//glFrustum(-ar, ar, -ar, ar, 0.0, 40.0);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	do {
		//Mueve camara
		if (glfwGetKey(window, GLFW_KEY_RIGHT) == GLFW_PRESS)
		{
			arma::fcolvec transeye = { {eye[0]}, {eye[1]}, {eye[2]} ,{1.0} };
			arma::fmat rotarcamara = Tr.R(0.0, 1.0, 0.0, -5.0);
			arma::fcolvec res = rotarcamara*transeye;
			eye = { res[0], res[1], res[2] };
			DOP = arma::normalise(camera - eye);

		}
		if (glfwGetKey(window, GLFW_KEY_LEFT) == GLFW_PRESS)
		{
			arma::fcolvec transeye = { {eye[0]}, {eye[1]}, {eye[2]} ,{1.0} };
			arma::fmat rotarcamara = Tr.R(0.0, 1.0, 0.0, 5.0);
			arma::fcolvec res = rotarcamara * transeye;
			eye = { res[0], res[1], res[2] };
			DOP = arma::normalise(camera - eye);

		}
		if (glfwGetKey(window, GLFW_KEY_UP) == GLFW_PRESS)
		{
			arma::fcolvec transeye = { {eye[0]}, {eye[1]}, {eye[2]} ,{1.0} };
			float angle = 2.0;
			if (angley >= 76.0) {
				angle = 0.0;
			}
			angley += angle;
			arma::fmat rotarcamara = Tr.R(0.0, 0.0, 1.0, angle);
			arma::fcolvec res = rotarcamara * transeye;
			eye = { res[0], res[1], res[2] };
			DOP = arma::normalise(camera - eye);

		}
		if (glfwGetKey(window, GLFW_KEY_DOWN) == GLFW_PRESS)
		{
			arma::fcolvec transeye = { {eye[0]}, {eye[1]}, {eye[2]} ,{1.0} };
			float angle = -2.0;
			if (angley <= -50.0) {

				angle = 0.0;
			}
			angley += angle;
			arma::fmat rotarcamara = Tr.R(0.0, 0.0, 1.0, angle);
			arma::fcolvec res = rotarcamara * transeye;
			eye = { res[0], res[1], res[2] };
			DOP = arma::normalise(camera - eye);

		}

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		gluLookAt(eye[0], eye[1], eye[2],
			camera[0], camera[1], camera[2],
			0.0, 1.0, 0.0);

		//ground
		vector<Vertex> ground_vertices;
		Vertex vg = Vertex(-9, 0, -5);
		ground_vertices.push_back(vg);
		vg = Vertex(9, 0, -5);
		ground_vertices.push_back(vg);
		vg = Vertex(9, -0.1, 5);
		ground_vertices.push_back(vg);
		vg = Vertex(-9, -0.1, 5);
		ground_vertices.push_back(vg);

		glBegin(GL_QUADS);
		glColor3f(0, 0.2, 0);
		for (int i = 0; i < 4; i++)
		{
			arma::frowvec vert = ground_vertices[i].getVertex();
			glVertex3f(vert[0], vert[1], vert[2]);

		}
		glEnd();

		// Dibujar el pokemon

		vector<Face> visibles_pokemon = pokemon.iluminacion(DOP);
		vector<arma::frowvec> colores = pokemon.colorea(visibles_pokemon);
		glBegin(GL_TRIANGLES);
		for (unsigned int i = 0; i < visibles_pokemon.size(); i++) {
			glColor3f(colores[i][0], colores[i][1], colores[i][2]);
			for (int j = 0; j < 3; j++) {
				arma::frowvec vert = visibles_pokemon[i].vertices[j].getVertex();
				glVertex3f(vert[0], vert[1], vert[2]);
			}

		}
		glEnd();
		// Fin dibujado del pokemon

		// Dibujar la pokebola[0].vertices[0].getVertex();
		posicion_pokemon = pokemon.faces[200].vertices[0].getVertex();
		std::vector< Vertex > pokebola_vertices = pokebola.lanza(transf, pokemon.radio, posicion_pokemon);

		glColor3f(pokebola.color[0], pokebola.color[1], pokebola.color[2]);
		glBegin(GL_TRIANGLES);
		for (unsigned int i = 0; i < pokebola_vertices.size(); i++) {
			arma::frowvec vert = pokebola_vertices[i].getVertex();
			glVertex3f(vert[0], vert[1], vert[2]);
		}
		glEnd();
		// Fin dibujado de la pokebola

			//Checa si lo atrapo
			if (pokebola.atrapa)
			{
				std::cout << "ATRAPADO" << endl;

				/*glfwTerminate();
				fflush(stdin);
				getchar();

				return 0;*/
			}

		glfwSwapBuffers(window);
		glfwPollEvents();

	} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
		glfwWindowShouldClose(window) == 0);

	glfwTerminate();

	return 0;
}

ext: cpp

max_stars_repo_path: Bota Pelota.cpp

max_stars_repo_name: zeunix/Bota-Pelota-

max_issues_repo_path: Bota Pelota.cpp

max_issues_repo_name: zeunix/Bota-Pelota-

max_forks_repo_path: Bota Pelota.cpp

max_forks_repo_name: zeunix/Bota-Pelota-


text:
#include "gtest/gtest.h"
#include <sstream>
#include <iostream>
#include <boost/program_options.hpp>
#include "hts_Primers.h"

namespace po = boost::program_options;

class Primer : public ::testing::Test {
public:
    po::variables_map vm;
    const std::string primer_1 = "TTCATTAAAAATTGAATTGACATTAACCT";
    const std::string read_seq_1 = "CGGGTTTCATTAAAAATTGAATTGACATTAACCTATAAAAATAGGCGTCGAGGCCCTTTCGTCTTCTATCGGAGCTCCAAGACCGCCTCGGCGTGAAGGTGGTGATAGCGCCCGGAAGAGAGTCAATTCAGGGTGGTGAATACTCTAGATC";

    const std::string primer_5p = "GTGYCAGCMGCCGCGGTAA";
    const std::string primer_3p = "GGACTACNVGGGTWTCTAAT";
    const std::string read_seq_1f = "@M02326:75:000000000-BL25H:1:1103:15116:4927 1:N:0:CTGCCATA+CTGCCATA\nGTAGGGTGTCAGCCGCCGCGGTAATACGAAGGTGGCAAGCGTTGTTCGGATTTACTGGGCGTAAAGGGAGCGTAGGTGGTTAGGTAAGCCCTCCGGGAAATCTTCAGGCTTAACCTGAAAAGGTCGGGGGGGACTGCCTAGCTAGAGGGCGGGAGAGGAGCGCGGAATTCCCGGTGTAGCGGTGAAATGCGTAGAGATCGGGAGGAAGGCCGGTGGCGAAGGCGGCGCTCTGGAACGTTTCTGACACTGAGGCTCGGAAGCGTGGGGAGCAAACAGGATTAGAAACCCCTGTAGTCCGTAG\n+\nCCCCCGGGGGGGFGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGEGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGFGGFGGGGEGGGGGGGGGGGGGGGGGFFGGGGGGGGGGGGGGGGGGGGGGGGGGGEGCGGGGGGGGGGGGGGGGGGGGGGGGGGGGEGDGFGGGGGGGGGGGGGGGGGEGGCGGGGGGGGFGGGGGGFGGFFGFGGCFGGC?FGGGGGFFFFFFFFF:FFD?BFFFFF1?FF::D<FABB:<B4";
    const std::string read_seq_1r = "@M02326:75:000000000-BL25H:1:1103:15116:4927 2:N:0:CTGCCATA+CTGCCATA\nACGGACTACAGGGGTTTCTAATCCTGTTTGCTCCCCACGCTTCCGAGCCTCAGTGTCAGAAACGTTCCAGAGCGCCGCCTTCGCCACCGGCCTTCCTCCCGATCTCTACGCATTTCACCGCTACACCGGGAATTCCGCGCTCCTCTCCCGCCCTCTAGCTAGGCAGTCCCCCCCGACCTTTTCAGGTTAAGCCTGAAGATTTCCCGGAGGGCTTACCTAACCACCTACGCTCCCTTTACGCCCAGTAAATCCGAACAACGCTTGCCACCTTCGTATTACCCCGGCGGCTGACACCCTACAC\n+\nCCCCCGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGDGGGGGEGGGGGGGGGGGGGGGGGGGECFEGGGGDGGGGGGGGGGGGDGGGGGGGFGGGGGDGGGFGGGGGGGGGGGGGGGDFGGGGGCFFDEDEEDGGGGGF7@FFDGDDGFEFFEFGDFGGDFFGGGGEEDEGDGFFFFGFC>ECGGGFG4>?CFGF6AFGDD<5>>FF<?FGBFGFF7?FFFBB>15(8?2:<<<5@1?:?FFF2249?FB;?<(><CF>FC?FF>>328:FA24(32,497>>63*19<51:0,(";
    const std::string read_seq_2f = "@M02326:75:000000000-BL25H:1:1103:24744:3455 1:N:0:CTGCCATA+CTGCCATA\nGTCTTGTGTCAGCCGCCGCGTTAATACAGAGGTCCCAAGCGTTGTTCGGATTCATTGGGCGTAAAGGGTGCGTAGGTGGCGCCGTAAGTGGGGTGTGAAATTTCGGAGCTTAACTCCGAAACTGCATTCCATACTGCGGTGCTTGAGGACTGGAGAGGAGACCGGAATTCATGGTGTAGCAGTGAAATGCGTAGAGATCATGAGGAAGACCAGTGGCGAAGGCGGGTCTCTGGACAGTTCCTGACACTGAGGCACGAAGGCTAGGGGAGCGAACGGGATTAGATACCCTAGTAGTCCAGAA\n+\nC@BCCFDEGGGGGGGGGGGGGEGECFGGGGGGGCCGCEFGEFGGGFFEGD7FEFGGGGGGG@CCGFEEGGGGFEGGGGGDGGFG@C@FBA@FGCCF?<@<<EGGGGGGGFGGGGG<=7FGDGGEAFFFFGGGFFD<F:CFFEGCEGGGGFE:FGDFGGFGGGCFGGEG7CFGGGGGFFGFGGGFFFFFF9FEC<8FF9?F7<BCECGGFCB@FCFGC1C>8*CCCB83*+:CCCC6>?7CCFCD<476?GFFFGGDGC?@C*CFFF7;;@DEFFFFBD3:D3==>))47F1AAFA9<48<(";
    const std::string read_seq_2r = "@M02326:75:000000000-BL25H:1:1103:24744:3455 2:N:0:CTGCCATA+CTGCCATA\nGGACTACTAGGGTATCTAATCCCGTTCGCTCCCCTAGCCTTCGTGCCTCAGTGTCAGGAACTGTCCAGAGACCCGCCTTCGCCACTGGTCTTCCTCATGATCTCTACGCATTTCACTGCTACACCATGAATTCCGGTCTCCTCTCCAGTCCTCAAGCACCGCAGTATGGAATGCAGTTTCGGAGTTAAGCTCCGAAATTTCACACCCCACTTACGGCGCCACCTACGCACCCTTTACGCCCAATGAATCCGAACAACGCTTGGGCCATCTGTATCACCGCGGCGGCTGACACAGACCACAA\n+\nC@CCCFFGGFFGGGGEGGFFGEGEGGGGG,F@FGGGG?F@FEFEFG@,EF<,EFFCEGEFEFEGGGG<FGFGDG:7C,@FFCB8FGG8FF9E?FGGC9EEFCGGCFCBCEC:FGEG9<FAA;FG8?B<F9B,949+6@DGGGFFFFG8@9D8DFG9F,==1>C@EG,3==@BEA?DA9EF5E8?6,@E7,25?:6+C?CEEFF7;9DGF>)87C5/;)8C86(56=@29@@DFD8)66(77518@;;CC=1(,-4(400(0,*))0*(5:4<2-))-,-(,42,21(3(4?FF*(1(.(((";
    const std::string read_seq_3f = "@M02326:75:000000000-BL25H:1:2113:25713:12283 1:N:0:GCATCCTA+GCATCCTA\nTGGACTACTGGGGTATCTAATCCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTCCGCCACATAAATCCGAACAACGCTTGCCCCCTCCGTATTACCTCGGCGGCTGCCACCGACCGGA\n+\nCCCCCFGGG@<FGGGGGGGEGFFGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGEGGGFGGGGFGGGGGGEGGFEFGGGGGFGGGGGGGGGF9FEGGGFF@ECGDGGFGGGGFGGGGFDGFFGGGGF,EFCGFDDDFDDFGFDFGFEFFGFDF9EC8@ADDGGGGFGGGFGFGGGF7D@C*>:@@,3@EDDFAFGA;@FFBFE>BCF6EBCF?E@A>D5=>-5).65?))678)(/=)).58@>0611086(/1*;+*1(0(49?(,4442)5:1)21(4(((2*/(((-2((";
    const std::string read_seq_3r = "@M02326:75:000000000-BL25H:1:2113:25713:12283 2:N:0:GCATCCTA+GCATCCTA\nGTGTCAGCCGCCGCGGTAATACGGAGGGTGCAAGCGTTGTTCGGATTTATTGGGCGTAAAGCGCGTGTAGGCGGTTTTCTAAGTCTGATGTGAAAGCCCTGGGCTCAACCCAGGAAGTGCATTGGATACTGGGAGACTTGAATACGGGAGAGGGTAGTGGAATTCCTAGTGTAGGAGTGAAATCCGTAGATATTAGGAGGAACACCGGTGGCGAAGGCGGCTACCTGGACCGATATTGACGCTGAGACGCGAAAGCGTGGGGAGCAAACAGGATTAGATACCCCAGTAGTCCAAGATCGGA\n+\nCCCCCGGGGGGDEDGGGGGGGGGG7FGGFGGGFGGGGGGGGGGGGGFGGGGFGGGGGGGGGGGGGGGGGGGGGGGGGGGGGAFFGFGGGGGGGGGGGFGGGGGGGGGGGGGGGGDEGFGGGGGGGFGGGGGGGGGDFGGGGFGGGGGGGF>FGEGGGGGFGEGGFEGGGGGGGGGC7FGGFGGGGDG7EEGGGGGGGEGGGGGFGGGGGGGGG5BBE>EGGGGGGGGFFFGEGC=EGFGFEGGGGGGF3@D=7:EFGGFGD3CC>F4::BGFFFFF?FAABF7<16F<><BAFFF?@ABF3";
    const std::string read_seq_se = "@M02326:75:000000000-BL25H:1:2113:25713:12283 1:N:0:GCATCCTA+GCATCCTA\nGTGTCAGCCGCCGCGGTAATACGGAGGGTGCAAGCGTTGTTCGGATTTATTGGGCGTAAAGCGCGTGTAGGCGGTTTTCTAAGTCTGATGTGAAAGCCCTGGGCTCAACCCAGGAAGTGCATTGGATACTGGGAGACTTGAATACGGGAGAGGGTAGTGGAATTCCTAGTGTAGGAGTGAAATCCGTAGATATTAGGAGGAACACCGGTGGCGAAGGCGGCTACCTGGACCGATATTGACGCTGAGACGCGAAAGCGTGGGGAGCAAACAGGATTAGATACCCCAGTAGTCCA\n+\nIII<IIIIIIIII<IIIIIIIIIIFIII<IIIIIIIIIIIIIIIIIIIII@>IIII?IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII";
    Primers p;
};

TEST_F(Primer, test_bounded_edit_distance) {
    ALIGNPOS r;
    // primer, seq, float_bp, max_error, end_matches
    r = p.bounded_edit_distance(primer_1, read_seq_1, 6, 4, 4);

    ASSERT_EQ(0u, r.dist);
    ASSERT_EQ(5u, r.spos);
    ASSERT_EQ(34u, r.epos);
};

TEST_F(Primer, test_pairs_both_match) {
    po::variables_map vm;

    std::istringstream in1(read_seq_1f);
    std::istringstream in2(read_seq_1r);
    InputReader<PairedEndRead, PairedEndReadFastqImpl> ifp(in1, in2);
    SeqMap primer5p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer3p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifp.has_next()) {
        auto i = ifp.next();
        PairedEndRead* per = dynamic_cast<PairedEndRead*>(i.get());
        counter.input(*per);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_pe(*per, counter, primer5p, primer3p, 4, 4, 5, true, false, 2);
        counter.output(*per);
        ASSERT_EQ("TACGAAGGTGGCAAGCGTTGTTCGGATTTACTGGGCGTAAAGGGAGCGTAGGTGGTTAGGTAAGCCCTCCGGGAAATCTTCAGGCTTAACCTGAAAAGGTCGGGGGGGACTGCCTAGCTAGAGGGCGGGAGAGGAGCGCGGAATTCCCGGTGTAGCGGTGAAATGCGTAGAGATCGGGAGGAAGGCCGGTGGCGAAGGCGGCGCTCTGGAACGTTTCTGACACTGAGGCTCGGAAGCGTGGGGAGCAAACAGGATTAGAAACCCCTGTAGTCCGTAG", (per->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(24u, (per->non_const_read_one()).getLTrim());
        ASSERT_EQ("CCTGTTTGCTCCCCACGCTTCCGAGCCTCAGTGTCAGAAACGTTCCAGAGCGCCGCCTTCGCCACCGGCCTTCCTCCCGATCTCTACGCATTTCACCGCTACACCGGGAATTCCGCGCTCCTCTCCCGCCCTCTAGCTAGGCAGTCCCCCCCGACCTTTTCAGGTTAAGCCTGAAGATTTCCCGGAGGGCTTACCTAACCACCTACGCTCCCTTTACGCCCAGTAAATCCGAACAACGCTTGCCACCTTCGTATTACCCCGGCGGCTGACACCCTACAC", (per->non_const_read_two()).get_sub_seq());
        ASSERT_EQ(22u, (per->non_const_read_two()).getLTrim());
        ASSERT_EQ(0u, counter.flipped);
    }
};

TEST_F(Primer, test_pairs_flipped) {
    std::istringstream in1(read_seq_3f);
    std::istringstream in2(read_seq_3r);
    InputReader<PairedEndRead, PairedEndReadFastqImpl> ifp(in1, in2);
    SeqMap primer5p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer3p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifp.has_next()) {
        auto i = ifp.next();
        PairedEndRead* per = dynamic_cast<PairedEndRead*>(i.get());
        counter.input(*per);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_pe(*per, counter, primer5p, primer3p, 4, 4, 5, true, false, 2);
        counter.output(*per);
        ASSERT_EQ("TACGGAGGGTGCAAGCGTTGTTCGGATTTATTGGGCGTAAAGCGCGTGTAGGCGGTTTTCTAAGTCTGATGTGAAAGCCCTGGGCTCAACCCAGGAAGTGCATTGGATACTGGGAGACTTGAATACGGGAGAGGGTAGTGGAATTCCTAGTGTAGGAGTGAAATCCGTAGATATTAGGAGGAACACCGGTGGCGAAGGCGGCTACCTGGACCGATATTGACGCTGAGACGCGAAAGCGTGGGGAGCAAACAGGATTAGATACCCCAGTAGTCCAAGATCGGA", (per->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(19u, (per->non_const_read_one()).getLTrim());
        ASSERT_EQ("CCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTCCGCCACATAAATCCGAACAACGCTTGCCCCCTCCGTATTACCTCGGCGGCTGCCACCGACCGGA", (per->non_const_read_two()).get_sub_seq());
        ASSERT_EQ(21u, (per->non_const_read_two()).getLTrim());
        ASSERT_EQ(1u, counter.flipped);
    }
};

TEST_F(Primer, test_pairs_one_fail) {
    std::istringstream in1(read_seq_2f);
    std::istringstream in2(read_seq_2r);
    InputReader<PairedEndRead, PairedEndReadFastqImpl> ifp(in1, in2);
    SeqMap primer5p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer3p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifp.has_next()) {
        auto i = ifp.next();
        bool test;
        PairedEndRead* per = dynamic_cast<PairedEndRead*>(i.get());
        counter.input(*per);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        test = p.check_read_pe(*per, counter, primer5p, primer3p, 4, 4, 5, true, false, 2);
        ASSERT_EQ(false, test);
        ASSERT_EQ(1u, counter.PE_Discarded);
    }
};

TEST_F(Primer, test_pairs_one_fail_ok) {
    std::istringstream in1(read_seq_2f);
    std::istringstream in2(read_seq_2r);
    InputReader<PairedEndRead, PairedEndReadFastqImpl> ifp(in1, in2);
    SeqMap primer5p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer3p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifp.has_next()) {
        auto i = ifp.next();
        PairedEndRead* per = dynamic_cast<PairedEndRead*>(i.get());
        counter.input(*per);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_pe(*per, counter, primer5p, primer3p, 4, 4, 5, true, false, 1);
        counter.output(*per);
        ASSERT_EQ("GTCTTGTGTCAGCCGCCGCGTTAATACAGAGGTCCCAAGCGTTGTTCGGATTCATTGGGCGTAAAGGGTGCGTAGGTGGCGCCGTAAGTGGGGTGTGAAATTTCGGAGCTTAACTCCGAAACTGCATTCCATACTGCGGTGCTTGAGGACTGGAGAGGAGACCGGAATTCATGGTGTAGCAGTGAAATGCGTAGAGATCATGAGGAAGACCAGTGGCGAAGGCGGGTCTCTGGACAGTTCCTGACACTGAGGCACGAAGGCTAGGGGAGCGAACGGGATTAGATACCCTAGTAGTCCAGAA", (per->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(0u, (per->non_const_read_one()).getLTrim());
        ASSERT_EQ(0u, (per->non_const_read_one()).getRTrim());
        ASSERT_EQ("CCCGTTCGCTCCCCTAGCCTTCGTGCCTCAGTGTCAGGAACTGTCCAGAGACCCGCCTTCGCCACTGGTCTTCCTCATGATCTCTACGCATTTCACTGCTACACCATGAATTCCGGTCTCCTCTCCAGTCCTCAAGCACCGCAGTATGGAATGCAGTTTCGGAGTTAAGCTCCGAAATTTCACACCCCACTTACGGCGCCACCTACGCACCCTTTACGCCCAATGAATCCGAACAACGCTTGGGCCATCTGTATCACCGCGGCGGCTGACACAGACCACAA", (per->non_const_read_two()).get_sub_seq());
        ASSERT_EQ(20u, (per->non_const_read_two()).getLTrim());
        ASSERT_EQ(0u, (per->non_const_read_two()).getRTrim());
        ASSERT_EQ(0u, counter.flipped);
    }
};

TEST_F(Primer, test_pairs_flipped_5p_match_only) {
    std::istringstream in1(read_seq_3f);
    std::istringstream in2(read_seq_3r);
    InputReader<PairedEndRead, PairedEndReadFastqImpl> ifp(in1, in2);
    SeqMap primer5p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer3p;

    PrimerCounters counter("hts_Primers", vm);
    while(ifp.has_next()) {
        auto i = ifp.next();
        PairedEndRead* per = dynamic_cast<PairedEndRead*>(i.get());
        counter.input(*per);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_pe(*per, counter, primer5p, primer3p, 4, 4, 5, true, false, 1);
        counter.output(*per);
        ASSERT_EQ("TACGGAGGGTGCAAGCGTTGTTCGGATTTATTGGGCGTAAAGCGCGTGTAGGCGGTTTTCTAAGTCTGATGTGAAAGCCCTGGGCTCAACCCAGGAAGTGCATTGGATACTGGGAGACTTGAATACGGGAGAGGGTAGTGGAATTCCTAGTGTAGGAGTGAAATCCGTAGATATTAGGAGGAACACCGGTGGCGAAGGCGGCTACCTGGACCGATATTGACGCTGAGACGCGAAAGCGTGGGGAGCAAACAGGATTAGATACCCCAGTAGTCCAAGATCGGA", (per->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(19u, (per->non_const_read_one()).getLTrim());
        ASSERT_EQ("TGGACTACTGGGGTATCTAATCCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTCCGCCACATAAATCCGAACAACGCTTGCCCCCTCCGTATTACCTCGGCGGCTGCCACCGACCGGA", (per->non_const_read_two()).get_sub_seq());
        ASSERT_EQ(0u, (per->non_const_read_two()).getLTrim());
        ASSERT_EQ(1u, counter.flipped);
    }
};

TEST_F(Primer, test_pairs_flipped_3p_match_only) {
    std::istringstream in1(read_seq_3f);
    std::istringstream in2(read_seq_3r);
    InputReader<PairedEndRead, PairedEndReadFastqImpl> ifp(in1, in2);
    SeqMap primer5p;
    SeqMap primer3p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifp.has_next()) {
        auto i = ifp.next();
        PairedEndRead* per = dynamic_cast<PairedEndRead*>(i.get());
        counter.input(*per);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_pe(*per, counter, primer5p, primer3p, 4, 4, 5, true, false, 1);
        counter.output(*per);
        ASSERT_EQ("GTGTCAGCCGCCGCGGTAATACGGAGGGTGCAAGCGTTGTTCGGATTTATTGGGCGTAAAGCGCGTGTAGGCGGTTTTCTAAGTCTGATGTGAAAGCCCTGGGCTCAACCCAGGAAGTGCATTGGATACTGGGAGACTTGAATACGGGAGAGGGTAGTGGAATTCCTAGTGTAGGAGTGAAATCCGTAGATATTAGGAGGAACACCGGTGGCGAAGGCGGCTACCTGGACCGATATTGACGCTGAGACGCGAAAGCGTGGGGAGCAAACAGGATTAGATACCCCAGTAGTCCAAGATCGGA", (per->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(0u, (per->non_const_read_one()).getRTrim());
        ASSERT_EQ("CCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTCCGCCACATAAATCCGAACAACGCTTGCCCCCTCCGTATTACCTCGGCGGCTGCCACCGACCGGA", (per->non_const_read_two()).get_sub_seq());
        ASSERT_EQ(21u, (per->non_const_read_two()).getLTrim());
        ASSERT_EQ(1u, counter.flipped);
    }
};

TEST_F(Primer, test_single_match_only) {
    std::istringstream in1(read_seq_se);
    InputReader<SingleEndRead, SingleEndReadFastqImpl> ifs(in1);
    SeqMap primer5p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer3p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifs.has_next()) {
        auto i = ifs.next();
        SingleEndRead* ser = dynamic_cast<SingleEndRead*>(i.get());
        counter.input(*ser);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_se(*ser, counter, primer5p, primer3p, 4, 4, 5, true, false, 2);
        counter.output(*ser);
        ASSERT_EQ("TACGGAGGGTGCAAGCGTTGTTCGGATTTATTGGGCGTAAAGCGCGTGTAGGCGGTTTTCTAAGTCTGATGTGAAAGCCCTGGGCTCAACCCAGGAAGTGCATTGGATACTGGGAGACTTGAATACGGGAGAGGGTAGTGGAATTCCTAGTGTAGGAGTGAAATCCGTAGATATTAGGAGGAACACCGGTGGCGAAGGCGGCTACCTGGACCGATATTGACGCTGAGACGCGAAAGCGTGGGGAGCAAACAGG", (ser->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(19u, (ser->non_const_read_one()).getLTrim());
        ASSERT_EQ(21u, (ser->non_const_read_one()).getRTrim());
        ASSERT_EQ(0u, counter.flipped);
    }
};

TEST_F(Primer, test_single_match_flip) {
    std::istringstream in1(read_seq_se);
    InputReader<SingleEndRead, SingleEndReadFastqImpl> ifs(in1);
    SeqMap primer3p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer5p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifs.has_next()) {
        auto i = ifs.next();
        SingleEndRead* ser = dynamic_cast<SingleEndRead*>(i.get());
        counter.input(*ser);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_se(*ser, counter, primer5p, primer3p, 4, 4, 5, true, false, 2);
        counter.output(*ser);
        ASSERT_EQ("CCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTACGCCCAATAAATCCGAACAACGCTTGCACCCTCCGTA", (ser->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(21u, (ser->non_const_read_one()).getLTrim());
        ASSERT_EQ(19u, (ser->non_const_read_one()).getRTrim());
        ASSERT_EQ(1u, counter.flipped);
    }
};

TEST_F(Primer, test_single_match_flip_5p_only) {
    std::istringstream in1(read_seq_se);
    InputReader<SingleEndRead, SingleEndReadFastqImpl> ifs(in1);
    SeqMap primer3p;
    SeqMap primer5p = p.fasta2dict(primer_3p, "seq");

    PrimerCounters counter("hts_Primers", vm);
    while(ifs.has_next()) {
        auto i = ifs.next();
        SingleEndRead* ser = dynamic_cast<SingleEndRead*>(i.get());
        counter.input(*ser);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_se(*ser, counter, primer5p, primer3p, 4, 4, 5, true, false, 1);
        counter.output(*ser);
        ASSERT_EQ("CCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTACGCCCAATAAATCCGAACAACGCTTGCACCCTCCGTATTACCGCGGCGGCTGACAC", (ser->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(21u, (ser->non_const_read_one()).getLTrim());
        ASSERT_EQ(0u, (ser->non_const_read_one()).getRTrim());
        ASSERT_EQ(1u, counter.flipped);
    }
};

TEST_F(Primer, test_single_match_flip_3p_only) {
    std::istringstream in1(read_seq_se);
    InputReader<SingleEndRead, SingleEndReadFastqImpl> ifs(in1);
    SeqMap primer3p = p.fasta2dict(primer_5p, "seq");
    SeqMap primer5p;

    PrimerCounters counter("hts_Primers", vm);
    while(ifs.has_next()) {
        auto i = ifs.next();
        SingleEndRead* ser = dynamic_cast<SingleEndRead*>(i.get());
        counter.input(*ser);
        // check_read_pe(PairedEndRead &pe, PrimerCounters &counter, SeqMap &primer5p, SeqMap &primer3p, const size_t pMismatches, const size_t pEndMismatches, const size_t pfloat, const size_t flip, const size_t keep, const size_t mpmatches
        p.check_read_se(*ser, counter, primer5p, primer3p, 4, 4, 5, true, false, 1);
        counter.output(*ser);
        ASSERT_EQ("TGGACTACTGGGGTATCTAATCCTGTTTGCTCCCCACGCTTTCGCGTCTCAGCGTCAATATCGGTCCAGGTAGCCGCCTTCGCCACCGGTGTTCCTCCTAATATCTACGGATTTCACTCCTACACTAGGAATTCCACTACCCTCTCCCGTATTCAAGTCTCCCAGTATCCAATGCACTTCCTGGGTTGAGCCCAGGGCTTTCACATCAGACTTAGAAAACCGCCTACACGCGCTTTACGCCCAATAAATCCGAACAACGCTTGCACCCTCCGTA", (ser->non_const_read_one()).get_sub_seq());
        ASSERT_EQ(0u, (ser->non_const_read_one()).getLTrim());
        ASSERT_EQ(19u, (ser->non_const_read_one()).getRTrim());
        ASSERT_EQ(1u, counter.flipped);
    }
};

max_issues_repo_name: ibest/HTStream

max_forks_repo_name: ibest/HTStream


text:
//
// Copyright (c) 2019 Singular Inversions Inc. (facegen.com)
// Use, modification and distribution is subject to the MIT License,
// see accompanying file LICENSE.txt or facegen.com/base_library_license.txt
//
// Test C++ behaviour
//

#include "stdafx.h"

#include <boost/any.hpp>

#include "FgCmd.hpp"
#include "FgSyntax.hpp"
#include "FgMetaFormat.hpp"
#include "FgImage.hpp"
#include "FgTestUtils.hpp"
#include "FgBuild.hpp"
#include "FgVersion.hpp"
#include "FgTime.hpp"

using namespace std;

namespace Fg {

static size_t rvoCount;

struct  FgTestmRvo
{
    Doubles      data;
    FgTestmRvo() {}
    FgTestmRvo(double v) : data(1,v) {}
    FgTestmRvo(const FgTestmRvo & rhs) : data(rhs.data) {++rvoCount; }
};

FgTestmRvo
fgTestmRvo()
{
    Doubles      t0(1,3.14159);
    return FgTestmRvo(t0[0]);
}

FgTestmRvo
fgTestmRvoMr(bool sel)
{
    Doubles      t0(1,2.71828),
                t1(1,3.14159);
    if (sel)
        return FgTestmRvo(t0[0]);
    else
        return FgTestmRvo(t1[0]);
}

FgTestmRvo
fgTestmNrvo(bool sel)
{
    Doubles      t0(1,2.71828),
                t1(1,3.14159);
    FgTestmRvo  ret;
    if (sel)
        ret = FgTestmRvo(t0[0]);
    else
        ret = FgTestmRvo(t1[0]);
    return ret;
}

FgTestmRvo
fgTestmNrvoMr(bool sel)
{
    Doubles      t0(1,2.71828),
                t1(1,3.14159);
    FgTestmRvo  ret;
    if (sel) {
        ret = FgTestmRvo(t0[0]);
        return ret;
    }
    else {
        ret = FgTestmRvo(t1[0]);
        return ret;
    }
}

static
void
rvo(const CLArgs &)
{
    FgTestmRvo      t;
    rvoCount = 0;
    t = fgTestmRvo();
    fgout << fgnl << "RVO copies: " << rvoCount;
    rvoCount = 0;
    t = fgTestmRvoMr(true);
    t = fgTestmRvoMr(false);
    fgout << fgnl << "RVO with multiple returns (both paths) copies: " << rvoCount;
    rvoCount = 0;
    t = fgTestmNrvo(true);
    t = fgTestmNrvo(false);
    fgout << fgnl << "NRVO copies (both assignment paths): " << rvoCount;
    rvoCount = 0;
    t = fgTestmNrvoMr(true);
    t = fgTestmNrvoMr(false);
    fgout << fgnl << "NRVO with multiple returns (both paths) copies: " << rvoCount;
}

static
void
speedExp(const CLArgs &)
{
    double      val = 0,
                inc = 2.718281828,
                mod = 3.141592653,
                acc = 0;
    size_t      reps = 10000000;
    FgTimer     tm;
    for (size_t ii=0; ii<reps; ++ii) {
        acc += exp(-val);
        val += inc;
        if (val > mod)
            val -= mod;
    }
    fgout << fgnl << "exp() time: " << 1000000.0 * tm.readMs() / reps << " ns  (dummy val: " << acc << ")";
}

static
void
fgexp(const CLArgs &)
{
    double      maxRel = 0,
                totRel = 0;
    size_t      cnt = 0;
    for (double dd=0; dd<5; dd+=0.001) {
        double  baseline = exp(-dd),
                test = expFast(-dd),
                meanVal = (test+baseline) * 0.5,
                relDel = (test-baseline) / meanVal;
        maxRel = cMax(maxRel,relDel);
        totRel += relDel;
        ++cnt;
    }
    fgout << "Max Rel Del: " << maxRel << " mean rel del: " << totRel / cnt;
    double      val = 0,
                inc = 2.718281828,
                mod = 3.141592653,
                acc = 0;
    size_t      reps = 10000000;
    FgTimer     tm;
    for (size_t ii=0; ii<reps; ++ii) {
        acc += expFast(-val);
        val += inc;
        if (val > mod)
            val -= mod;
    }
    fgout << fgnl << "exp() time: " << 1000000.0 * tm.readMs() / reps << " ns  (dummy val: " << acc << ")";
}

static
void
any(const CLArgs &)
{
    boost::any      v0 = 42,
                    v1 = v0;
    int *           v0Ptr = boost::any_cast<int>(&v0);
    *v0Ptr = 7;
    fgout << fgnl << "Original small value: " << *v0Ptr << " but copy remains at " << boost::any_cast<int>(v1);
    // Now try with a heavy object that is not subject to small value optimization (16 bytes) onto the stack:
    v0 = Mat44D(42);
    v1 = v0;
    Mat44D *      v0_ptr = boost::any_cast<Mat44D>(&v0);
    (*v0_ptr)[0] = 7;
    fgout << fgnl << "Original big value: " << (*v0_ptr)[0] << " but copy remains at " << boost::any_cast<Mat44D>(v1)[0];
}

static
void
thash(const CLArgs &)
{
    for (uint ii=0; ii<3; ++ii) {
        string              str = "Does this string give a consistent hash on subsequent runs ?";
        std::hash<string>   hfn;        // Returns size_t
#ifdef FG_64                            // size_t is 64 bits:
        fgout << fgnl << "64 bit hash: " << hfn(str);
#else                                   // size_t is 32 bits:
        size_t              hval32 = hfn(str);
        uint64              lo = hval32,
                            hi = hfn(str+toString(lo)),
                            hval64 = lo | (hi << 32);
        fgout << fgnl << "32 bit hash: " << hval32 << " and 64 composite: " << hval64;
#endif
    }
}

static
void
parr(const CLArgs &)
{
    // Generate data and put in both parallel and packed arrays:
    size_t          N = 100000,
                    A = 8;
    Doubless         pins(A),
                    pouts(A);
    for (Doubles & pin : pins)
        pin = fgGenerate<double>(randNormal,N);
    for (Doubles & pout : pouts)
        pout.resize(N,0);
    Doubles          sins,
                    souts(N*A,0.0);
    for (size_t ii=0; ii<N; ++ii)
        for (size_t jj=0; jj<A; ++jj)
            sins.push_back(pins[jj][ii]);

    // Parallel -> Parallel
    FgTimer         tm;
    for (size_t rr=0; rr<100; ++rr) {
        for (size_t ii=0; ii<N; ++ii) {
            pouts[0][ii] += pins[0][ii]*pins[1][ii] + pins[2][ii]*pins[3][ii];
            pouts[1][ii] += pins[1][ii]*pins[2][ii] + pins[3][ii]*pins[4][ii];
            pouts[2][ii] += pins[2][ii]*pins[3][ii] + pins[4][ii]*pins[5][ii];
            pouts[3][ii] += pins[3][ii]*pins[4][ii] + pins[5][ii]*pins[6][ii];
            pouts[4][ii] += pins[4][ii]*pins[5][ii] + pins[6][ii]*pins[7][ii];
            pouts[5][ii] += pins[5][ii]*pins[6][ii] + pins[7][ii]*pins[0][ii];
            pouts[6][ii] += pins[6][ii]*pins[7][ii] + pins[0][ii]*pins[1][ii];
            pouts[7][ii] += pins[7][ii]*pins[0][ii] + pins[1][ii]*pins[2][ii];
        }
    }
    size_t          time = tm.readMs();
    double          val = 0;
    for (const Doubles & outs : pouts)
        val += cSum(outs);
    fgout << fgnl << "Paral arrays in, paral arrays out: " << time << "ms. (" << val << ")";

    // Packed -> Packed
    tm.start();
    for (size_t rr=0; rr<100; ++rr) {
        for (size_t ii=0; ii<N; ++ii) {
            // Inlining this value in every use made no speed difference; the compiler
            // takes care of this automatically. We only leave it for clarity:
            size_t          idx = ii*8;
            souts[idx+0] += sins[idx+0]*sins[idx+1] + sins[idx+2]*sins[idx+3];
            souts[idx+1] += sins[idx+1]*sins[idx+2] + sins[idx+3]*sins[idx+4];
            souts[idx+2] += sins[idx+2]*sins[idx+3] + sins[idx+4]*sins[idx+5];
            souts[idx+3] += sins[idx+3]*sins[idx+4] + sins[idx+5]*sins[idx+6];
            souts[idx+4] += sins[idx+4]*sins[idx+5] + sins[idx+6]*sins[idx+7];
            souts[idx+5] += sins[idx+5]*sins[idx+6] + sins[idx+7]*sins[idx+0];
            souts[idx+6] += sins[idx+6]*sins[idx+7] + sins[idx+0]*sins[idx+1];
            souts[idx+7] += sins[idx+7]*sins[idx+0] + sins[idx+1]*sins[idx+2];
        }
    }
    time = tm.readMs();
    val = cSum(souts);
    fgout << fgnl << "Packed array in, packed array out: " << time << "ms. (" << val << ")";

    // Packed -> Parallel
    // I have no idea why this is faster than packed->packed. I looked at the MSVC17 x64 O2
    // disassembler (on Goldbolt using just 4 vals and a single mult) and the multiply, add, store
    // operands were identical ...
    for (Doubles & pout : pouts)
        fgFill(pout,0.0);
    tm.start();
    for (size_t rr=0; rr<100; ++rr) {
        for (size_t ii=0; ii<N; ++ii) {
            // Inlining this value in every use made no speed difference; the compiler
            // takes care of this automatically. We only leave it for clarity:
            size_t          idx = ii*8;
            pouts[0][ii] += sins[idx+0]*sins[idx+1] + sins[idx+2]*sins[idx+3];
            pouts[1][ii] += sins[idx+1]*sins[idx+2] + sins[idx+3]*sins[idx+4];
            pouts[2][ii] += sins[idx+2]*sins[idx+3] + sins[idx+4]*sins[idx+5];
            pouts[3][ii] += sins[idx+3]*sins[idx+4] + sins[idx+5]*sins[idx+6];
            pouts[4][ii] += sins[idx+4]*sins[idx+5] + sins[idx+6]*sins[idx+7];
            pouts[5][ii] += sins[idx+5]*sins[idx+6] + sins[idx+7]*sins[idx+0];
            pouts[6][ii] += sins[idx+6]*sins[idx+7] + sins[idx+0]*sins[idx+1];
            pouts[7][ii] += sins[idx+7]*sins[idx+0] + sins[idx+1]*sins[idx+2];
        }
    }
    time = tm.readMs();
    val = 0;
    for (const Doubles & outs : pouts)
        val += cSum(outs);
    fgout << fgnl << "Packed array in, paral arrays out: " << time << "ms. (" << val << ")";

}

void
fgCmdTestmCpp(const CLArgs & args)
{
    vector<Cmd>   cmds;
    cmds.push_back(Cmd(any,"any","Test boost any copy semantics"));
    cmds.push_back(Cmd(fgexp,"fgexp","Test and mesaure speed of interal optimized exp"));
    cmds.push_back(Cmd(thash,"hash","Test std::hash behaviour"));
    cmds.push_back(Cmd(parr,"parr","Test speedup of switching from parallel to packed arrays"));
    cmds.push_back(Cmd(rvo,"rvo","Return value optimization / copy elision"));
    cmds.push_back(Cmd(speedExp,"exp","Measure the speed of library exp(double)"));
    doMenu(args,cmds);
}

}

// */


text:
//
// Copyright (c) 2018-2019 Christian Mazakas (christian dot mazakas at gmail dot
// com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/LeonineKing1199/foxy
//

#include <foxy/client_session.hpp>
#include <foxy/log.hpp>

#include <boost/beast/version.hpp>
#include <boost/beast/http.hpp>
#include <boost/smart_ptr/make_unique.hpp>

#include <memory>

#include <catch2/catch.hpp>

using boost::system::error_code;
using boost::asio::ip::tcp;
namespace asio = boost::asio;
namespace http = boost::beast::http;
namespace ssl  = boost::asio::ssl;

using namespace std::chrono_literals;

TEST_CASE("Our SSL client session class")
{
  SECTION("should be able to asynchronously connect to a remote")
  {
    asio::io_context io;

    // create a client that uses TLS 1.2 and has a 30 second timeout
    //
    auto ctx = ssl::context(ssl::context::method::tlsv12_client);

    auto opts = foxy::session_opts{ctx, 30s};

    auto  session_handle = boost::make_unique<foxy::client_session>(io, opts);
    auto& session        = *session_handle;

    REQUIRE(session.stream.is_ssl());

    auto valid_request = false;

    session.async_connect(
      "www.google.com", "https",
      [&valid_request, &session, sh = std::move(session_handle)](error_code ec,
                                                                 tcp::endpoint) mutable -> void {
        auto parser_handle = boost::make_unique<http::response_parser<http::string_body>>();
        auto request_handle =
          boost::make_unique<http::request<http::empty_body>>(http::verb::get, "/", 11);

        auto& parser  = *parser_handle;
        auto& request = *request_handle;

        session.async_request(
          request, parser,
          [&valid_request, &session, &parser, &request, ph = std::move(parser_handle),
           rh = std::move(request_handle), sh = std::move(sh)](error_code ec) mutable -> void {
            auto response = parser.release();

            auto const is_valid_status = (response.result_int() == 200);
            auto const is_valid_body   = (response.body().size() > 0) &&
                                       boost::string_view(response.body()).ends_with("</html>");

            valid_request = is_valid_body && is_valid_status;

            // do this to prove that the TLS session is stable after a move
            //
            auto new_handler = boost::make_unique<foxy::client_session>(std::move(session));

            auto& other_session = *new_handler;

            other_session.stream.ssl().async_shutdown(
              [nh = std::move(new_handler)](error_code ec) -> void {
                if (ec == boost::asio::error::eof) {
                  // Rationale:
                  // http://stackoverflow.com/questions/25587403/boost-asio-ssl-async-shutdown-always-finishes-with-an-error
                  ec.assign(0, ec.category());
                }

                if (ec) { foxy::log_error(ec, "ssl client shutdown"); }
              });
          });
      });

    io.run();
    REQUIRE(valid_request);
  }

  SECTION("should timeout when the host can't be found")
  {
    asio::io_context io;

    auto ctx  = ssl::context(ssl::context::method::tlsv12_client);
    auto opts = foxy::session_opts{ctx, 250ms};

    auto  session_handle = boost::make_unique<foxy::client_session>(io, opts);
    auto& session        = *session_handle;

    auto timed_out = false;

    session.async_connect("www.google.com", "1337",
                          [&timed_out, &session, sh = std::move(session_handle)](
                            error_code ec, tcp::endpoint) mutable -> void {
                            timed_out = (ec == boost::asio::error::operation_aborted);
                          });

    io.run();
    REQUIRE(timed_out);
  }
}

max_stars_repo_name: djarek/foxy

max_issues_repo_name: djarek/foxy

max_forks_repo_name: djarek/foxy


text:
#include <iostream>
#include <fstream>
#include <string>
#include <vector>

// Note binary archive could not used because of crash at loading phase.
//#include <boost/archive/binary_iarchive.hpp>
//#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/filesystem.hpp>

#include <opencv2/opencv.hpp>

#define DARK_MAT_FILE (std::string("./dark.dat"))
#define DARK_FLOAT_MAT_FILE (std::string("./dark_float.dat"))
#define DARK_TIFF_FILE (std::string("./dark.tif"))

BOOST_SERIALIZATION_SPLIT_FREE(cv::Mat)
namespace boost {
	namespace serialization {

		template<class Archive>
		void save(Archive & ar, const cv::Mat& m, const unsigned int version) {
			size_t elemSize = m.elemSize(), elemType = m.type();

			ar & m.cols;
			ar & m.rows;
			ar & elemSize;
			ar & elemType;
			size_t dataSize = m.cols * m.rows * m.elemSize();

			for (size_t dc = 0; dc < dataSize; ++dc) {
				ar & m.data[dc];
			}
		}

		template<class Archive>
		void load(Archive & ar, cv::Mat& m, const unsigned int version) {
			int cols, rows;
			size_t elemSize, elemType;

			ar & cols;
			ar & rows;
			ar & elemSize;
			ar & elemType;

			m.create(rows, cols, elemType);
			size_t dataSize = m.cols * m.rows * elemSize;

			for (size_t dc = 0; dc < dataSize; ++dc) {
				ar & m.data[dc];
			}
		}
	}
}

void saveMat(cv::Mat& m, std::string filename) {
	std::ofstream ofs(filename.c_str());
	//boost::archive::binary_oarchive oa(ofs);
	boost::archive::text_oarchive oa(ofs);
	oa << m;
}

void loadMat(cv::Mat& m, std::string filename) {
	std::ifstream ifs(filename.c_str());
	//boost::archive::binary_iarchive ia(ifs);
	boost::archive::text_iarchive ia(ifs);
	ia >> m;
}

bool is_file_exist(std::string pPath) {
	boost::filesystem::path p (pPath);

	if(boost::filesystem::exists(p) && boost::filesystem::is_regular_file(p))
		return true;

	return false;
}

bool list_files(std::string pPath, std::vector<std::string> &pFileList) {
	std::cout << "Search Path:" << pPath << std::endl;

	pFileList.clear();
	boost::filesystem::path p (pPath);

	boost::filesystem::directory_iterator end_itr;

	for (boost::filesystem::directory_iterator itr(p); itr != end_itr; ++itr)
	{
		if (boost::filesystem::is_regular_file(itr->path())) {
			std::string current_file = itr->path().string();
			pFileList.push_back(itr->path().string());
		}
	}

	return true;
}

const cv::String keys =
	"{help h usage ? |      | show help            }"
	"{@image         |      | input image file     }"
	"{@out_image     |      | output image file    }"
	"{@dark_dir      |      | dark image folder    }"
	"{F f float      |      | do float calculation }"
	"{U u usedark    |      | use dark mat if exist}"
	"{D d savedark   |      | save dark image      }"
	"{S s show       |      | show result          }"
	;

int main (int argc,char *argv[]) {
    cv::CommandLineParser parser(argc, argv, keys);
    parser.about("A Simple dark frame subtraction program.");

	bool do_float = parser.has("F") ? true : false;
	bool use_darkmat = parser.has("U") ? true : false;
	bool save_dark = parser.has("D") ? true : false;
	bool do_show = parser.has("S") ? true : false;
	cv::String filename = parser.get<cv::String>("@image");
	cv::String outfilename = parser.get<cv::String>("@out_image");
	cv::String dark_dir = parser.get<cv::String>("@dark_dir");
	cv::String dark_mat_file = do_float ? cv::String(DARK_FLOAT_MAT_FILE) : cv::String(DARK_MAT_FILE);

	if (parser.has("h") || !parser.check() || filename.empty() || outfilename.empty() ||
		(!is_file_exist(dark_mat_file) && dark_dir.empty())) {
		parser.printMessage();
		return -1;
	}

	std::cout << "Processing file:" << filename << " float:" << do_float
		<< " output file:" << outfilename << " show result:" << do_show
		<< " use dark mat:" << use_darkmat << " dark_dir:" << dark_dir << std::endl;

	cv::Mat image = cv::imread(filename);

	if(image.empty()) {
		std::cout << "Error: No input image file." << std::endl;
		return -1;
	}

	cv::Mat dark_image;
	cv::Mat mat_sum;
	std::vector<std::string> dark_image_list;

	if(is_file_exist(dark_mat_file) && use_darkmat) { // dark Mat file found.
		try {
			std::cout << "Importing " << dark_mat_file << "..." << std::endl;
			loadMat(mat_sum,dark_mat_file);

		} catch(...) {
			std::cout << "Error: dark mat file \"" << DARK_MAT_FILE << "\" read failed." << std::endl;
			return -1;
		}
	} else { // make dark Mat from dark image files.
		if(do_float)
			mat_sum = cv::Mat::zeros(image.size(),CV_32FC3);
		else
			mat_sum = cv::Mat::zeros(image.size(),CV_16UC3);

		try {
			list_files(dark_dir,dark_image_list);
			std::cout << "Reading dark image files..." << std::endl;
			cv::Mat buf_mat;

			for(int i = 0;i < dark_image_list.size();++i) {
				cv::Mat dark_image;
				std::cout << dark_image_list[i] << std::endl;
				dark_image = cv::imread(dark_image_list[i]);
				if(dark_image.empty()) throw 1;

				if(do_float) {
					dark_image.convertTo(buf_mat, CV_32FC3, 1.0/255.);
					mat_sum += buf_mat;
				} else {
					dark_image.convertTo(buf_mat, CV_16UC3, 1);
					mat_sum += buf_mat;
				}
			}

			if(do_float) {
				mat_sum /= (dark_image_list.size());
			} else {
				mat_sum /= (float)(dark_image_list.size());
			}
			if(use_darkmat) saveMat(mat_sum, dark_mat_file);

		} catch(...) {
			std::cout << "Error: failed to read dark image files." << std::endl;
			return -1;
		}
	}

	if(do_float) {
		mat_sum.convertTo(dark_image, CV_8UC3, 255.);
	} else {
		mat_sum.convertTo(dark_image, CV_8UC3, 1.);
	}

	cv::Mat result_image = image - dark_image;
	if(save_dark) cv::imwrite(DARK_TIFF_FILE, dark_image);
	cv::imwrite(outfilename, result_image);

	if(do_show) {
		cv::namedWindow("original",cv::WINDOW_NORMAL);
		cv::imshow("original", image);
		cv::namedWindow("result",cv::WINDOW_NORMAL);
		cv::imshow("result", result_image);
		cv::namedWindow("dark",cv::WINDOW_NORMAL);
		cv::imshow("dark", dark_image);
		cv::waitKey(0);
	}

	return 0;
}


text:
/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_SKELETONS_PARAM_DEPS_NOTIFY_MAP_PD_HPP
#define STAPL_SKELETONS_PARAM_DEPS_NOTIFY_MAP_PD_HPP

#include <type_traits>
#include <stapl/utility/tuple/tuple.hpp>
#include <stapl/utility/tuple/front.hpp>
#include <stapl/skeletons/utility/utility.hpp>
#include <stapl/skeletons/operators/elem_helpers.hpp>
#include <stapl/skeletons/param_deps/utility.hpp>
#include <boost/utility/result_of.hpp>

namespace stapl {
namespace skeletons {
namespace skeletons_impl {

//////////////////////////////////////////////////////////////////////
/// @brief A notify_map parametric dependency is similar to a @c map_pd,
/// however, it waits for @c NotifCount notifications from the first
/// @c NotifCount flows.
///
/// Example - the inputs to a spawned element created by this skeleton
/// would be:
/// @li in<0>[idx]           {notification-only}
/// @li in<1>[idx]           {notification-only}
/// @li in<2>[idx]           {notification-only}
/// @li ...
/// @li in<NotifCount-1>[idx]{notifciation-only}
/// @li in<NotifCount>[idx]
///
/// @tparam Op          the workfunction to be applied on each element
/// @tparam NotifCount  the number of notification-only flows
///
/// @see map_pd
/// @see notify_map
///
/// @ingroup skeletonsParamDepsInternal
//////////////////////////////////////////////////////////////////////
template <typename Op, int NotifCount, typename F>
class notify_map_pd
  : public param_deps_defaults
{
  Op m_op;
  F  m_filter;

public:
  static constexpr std::size_t in_port_size = NotifCount + 1;
  static constexpr std::size_t op_arity     = 1;

  using op_type = Op;

  notify_map_pd(Op op, F f)
    : m_op(std::move(op)),
      m_filter(std::move(f))
  { }

  //////////////////////////////////////////////////////////////////////
  /// @brief If coord is <idx, ...> it wraps the @c Op with the
  /// following inputs and sends it to the visitor along with the
  /// @c m_op
  /// @li in<0>[idx]
  /// @li in<1>[idx]
  /// @li ...
  ///
  /// @param coord        <NotifCount, j, k, ...> where NotifCount < n,
  ///                     j < m, k < p
  /// @param visitor      the information about Op and input is passed
  ///                     so that later this information can be converted
  ///                     to a node in the dependence graph
  /// @param in_flow      a tuple of input flows to consume from
  //////////////////////////////////////////////////////////////////////
  template <typename Coord, typename Visitor, typename In>
  void case_of(Coord const& /*skeleton_size*/, Coord const& coord,
               Visitor& visitor, In&& in_flow) const
  {
    auto&& idx = tuple_ops::front(coord);

    apply_case_of(
      idx, visitor, std::forward<In>(in_flow),
      stapl::make_index_sequence<NotifCount>()
    );
  }

private:
  template <typename Index, typename Visitor, typename In,
            std::size_t... Indices>
  void apply_case_of(Index&& index, Visitor& visitor, In&& in_flow,
                     stapl::index_sequence<Indices...>&&) const
  {

    visitor(
      visitor.notification_list({
        stapl::get<Indices>(in_flow).depend_on(
          make_tuple(std::forward<Index>(index)))...}),
      m_op,
      no_mapper(),
      stapl::get<sizeof...(Indices)>(in_flow).consume_from(
        make_tuple(std::forward<Index>(index)), m_filter));
  }
public:

  //////////////////////////////////////////////////////////////////////
  /// @brief determines how many of the instances of this parametric
  /// dependency will be consuming from a producer with a given coordinate.
  /// This is a reverse query as compared to case_of
  ///
  /// @tparam FlowIndex the flow index to which this request is sent
  //////////////////////////////////////////////////////////////////////
  template <typename Size, typename Coord, typename FlowIndex>
  std::size_t consumer_count(Size const&  /*skeleton_size*/,
                             Coord const& /*producer_coord*/,
                             FlowIndex) const
  {
    return 1;
  }

public:
  Op get_op() const
  {
    return m_op;
  }

  F get_filter() const
  {
    return m_filter;
  }

  void define_type(typer& t)
  {
    t.member(m_op);
    t.member(m_filter);
  }
};

} // namespace skeletons_impl

//////////////////////////////////////////////////////////////////////
/// @brief Creates a zip parametric dependency given a @c op. This method
/// is used whenever filtering is needed on the input edge.
///
/// @copybrief skeletons_impl::notify_map_pd
///
/// @ingroup skeletonsParamDeps
//////////////////////////////////////////////////////////////////////
template <int NotifCount = 1,
          typename Op,
          typename F    = skeletons::no_filter>
skeletons_impl::notify_map_pd<Op, NotifCount, F>
notify_map_pd(Op const& op, F const& f = F())
{
  return skeletons_impl::notify_map_pd<Op, NotifCount, F>(op, f);
}

} // namespace skeletons
} // namespace stapl

#endif // STAPL_SKELETONS_PARAM_DEPS_NOTIFY_MAP_PD_HPP

ext: hpp


text:
/*
 *
 * Try
 * ledger-core
 *
 * Created by Pierre Pollastri on 13/12/2016.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 Ledger
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#ifndef LEDGER_CORE_TRY_HPP
#define LEDGER_CORE_TRY_HPP

#ifdef TARGET_JNI
#include <jni/jni/djinni_support.hpp>
#endif

#include <functional>
#include "optional.hpp"
#include "Option.hpp"
#include "Exception.hpp"
#include <typeinfo>
#include <stdexcept>
#include <boost/exception/diagnostic_information.hpp>
#include <utils/Try.hpp>

namespace ledger {
    namespace core {
        template <typename T>
        class Try {
        public:
            Try() {

            };

            Try(const T&v) {
                _value = optional<T>(v);
            }

            Try(api::ErrorCode code, const std::string& message) {
                fail(code, message);
            }

            void fail(api::ErrorCode code, const std::string& message) {
                _exception = Exception(code, message);
            }

            void fail(const Exception& ex) {
                _exception = ex;
            }

            void success(const T& v) {
                _value = v;
            }

            const T& getValue() const {
                return _value.value();
            }

            const Exception& getFailure() const {
                return _exception.value();
            }

            bool isFailure() const {
                return _exception ? true : false;
            }
            bool isSuccess() const {
                return _value ? true : false;
            }
            bool isComplete() const {
                return isSuccess() || isFailure();
            }

            Option<T> toOption() const {
                if (isSuccess())
                    return Option<T>(getValue());
                return Option<T>();
            }

            Option<Exception> exception() const {
                if (isFailure())
                    return Option<Exception>(getFailure());
                return Option<Exception>();
            }

            const T& getOrThrow() const {
                if (isFailure())
                    throw getFailure();
                return getValue();
            }

            template <class U>
            const T& getOrThrowException() const {
                if (isFailure())
                    throw U(getFailure().getMessage());
                return getValue();
            }

            Try<T> mapException(api::ErrorCode code) {
                if (isFailure())
                    return Try<T>(code, getFailure().getMessage());
                return *this;
            }

//            friend std::ostream &operator<<(std::ostream &os, const Try<T> &d) {
//                if (d.isSuccess()) {
//                    os << "Success(" << d.getFailure() << ")";
//                } else {
//                    os << "Failure(" << d.getFailure() << ")";
//                }
//            }

            ~Try() {

            };
        private:
            optional<Exception> _exception;
            optional<T> _value;

        public:
            static const Try<T> from(std::function<T ()> lambda) {
                Try<T> result;
                try {
                    result.success(lambda());
                } catch (const Exception& ex) {
                    result.fail(ex);
#ifdef TARGET_JNI
                } catch (const djinni::jni_exception& ex) {
                    result.fail(api::ErrorCode::RUNTIME_ERROR, ex.get_backtrace());
#endif
                } catch (...) {
                    result.fail(api::ErrorCode::RUNTIME_ERROR, boost::current_exception_diagnostic_information(true));
                }
                return result;
            }
        };

        template <typename T>
        using TryPtr = Try<std::shared_ptr<T>>;

        template<typename T>
        Try<T> make_try(std::function<T ()> lambda) {
            return Try<T>::from(lambda);
        }
    }
}

#endif //LEDGER_CORE_TRY_HPP

max_stars_repo_name: RomanWlm/lib-ledger-core

max_issues_repo_name: RomanWlm/lib-ledger-core

max_forks_repo_name: RomanWlm/lib-ledger-core


text:
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Fraunhofer IPA
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Fraunhofer IPA nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mathias Lüdtke */

#include <rclcpp/rclcpp.hpp>

#include <moveit/macros/class_forward.h>
#include <moveit/utils/rclcpp_utils.h>

#include <moveit_ros_control_interface/ControllerHandle.h>

#include <moveit/controller_manager/controller_manager.h>

#include <controller_manager_msgs/srv/list_controllers.hpp>
#include <controller_manager_msgs/srv/switch_controller.hpp>

#include <pluginlib/class_list_macros.hpp>
#include <pluginlib/class_loader.hpp>

#include <boost/bimap.hpp>

#include <map>
#include <memory>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.plugins.ros_control_interface");
static const rclcpp::Duration CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1.0);
static const double SERVICE_CALL_TIMEOUT = 1.0;

namespace moveit_ros_control_interface
{
/**
 * \brief Get joint name from resource name reported by ros2_control, since claimed_interfaces return by ros2_control
 * will have the interface name as suffix joint_name/INTERFACE_TYPE
 * @param[in] claimed_interface claimed interface as joint_name/INTERFACE_TYPE
 * @return joint_name part of the /p claimed_interface
 */
std::string parseJointNameFromResource(const std::string& claimed_interface)
{
  const auto index = claimed_interface.find('/');
  if (index == std::string::npos)
    return claimed_interface;
  return claimed_interface.substr(0, index);
}

MOVEIT_CLASS_FORWARD(MoveItControllerManager);  // Defines MoveItControllerManagerPtr, ConstPtr, WeakPtr... etc

/**
 * \brief moveit_controller_manager::MoveItControllerManager sub class that interfaces one ros_control
 * controller_manager
 * instance.
 * All services and names are relative to ns_.
 */
class MoveItControllerManager : public moveit_controller_manager::MoveItControllerManager
{
  std::string ns_;
  pluginlib::ClassLoader<ControllerHandleAllocator> loader_;
  typedef std::map<std::string, controller_manager_msgs::msg::ControllerState> ControllersMap;
  ControllersMap managed_controllers_;
  ControllersMap active_controllers_;
  typedef std::map<std::string, ControllerHandleAllocatorPtr> AllocatorsMap;
  AllocatorsMap allocators_;

  typedef std::map<std::string, moveit_controller_manager::MoveItControllerHandlePtr> HandleMap;
  HandleMap handles_;

  rclcpp::Time controllers_stamp_{ 0, 0, RCL_ROS_TIME };
  std::mutex controllers_mutex_;
  rclcpp::Node::SharedPtr node_;
  rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr list_controllers_service_;
  rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_service_;

  /**
   * \brief Check if given controller is active
   * @param s state of controller
   * @return true if controller is active
   */
  static bool isActive(const controller_manager_msgs::msg::ControllerState& s)
  {
    return s.state == std::string("active");
  }

  /**
   * \brief  Call list_controllers and populate managed_controllers_ and active_controllers_. Allocates handles if
   * needed.
   * Throttled down to 1 Hz, controllers_mutex_ must be locked externally
   * @param force force rediscover
   */
  void discover(bool force = false)
  {
    // Skip if controller stamp is too new for new discovery, enforce update if force==true
    if (!force && ((node_->now() - controllers_stamp_) < CONTROLLER_INFORMATION_VALIDITY_AGE))
      return;

    controllers_stamp_ = node_->now();

    auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>();
    auto result_future = list_controllers_service_->async_send_request(request);
    if (result_future.wait_for(std::chrono::duration<double>(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout)
    {
      RCLCPP_WARN_STREAM(LOGGER, "Failed to read controllers from " << list_controllers_service_->get_service_name()
                                                                    << " within " << SERVICE_CALL_TIMEOUT
                                                                    << " seconds");
      return;
    }

    managed_controllers_.clear();
    active_controllers_.clear();

    const auto& result = result_future.get();
    for (const controller_manager_msgs::msg::ControllerState& controller : result->controller)
    {
      if (isActive(controller))
      {
        auto& claimed_interfaces = active_controllers_.insert(std::make_pair(controller.name, controller))
                                       .first->second.claimed_interfaces;  // without namespace
        std::transform(claimed_interfaces.cbegin(), claimed_interfaces.cend(), claimed_interfaces.begin(),
                       [](const std::string& claimed_interface) {
                         return parseJointNameFromResource(claimed_interface);
                       });
      }
      if (loader_.isClassAvailable(controller.type))
      {
        std::string absname = getAbsName(controller.name);
        auto controller_it = managed_controllers_.insert(std::make_pair(absname, controller)).first;  // with namespace
        auto& claimed_interfaces = controller_it->second.claimed_interfaces;
        std::transform(claimed_interfaces.cbegin(), claimed_interfaces.cend(), claimed_interfaces.begin(),
                       [](const std::string& claimed_interface) {
                         return parseJointNameFromResource(claimed_interface);
                       });
        allocate(absname, controller_it->second);
      }
    }
  }

  /**
   * \brief Allocates a MoveItControllerHandle instance for the given controller
   * Might create allocator object first.
   * @param name fully qualified name of the controller
   * @param controller controller information
   */
  void allocate(const std::string& name, const controller_manager_msgs::msg::ControllerState& controller)
  {
    if (handles_.find(name) == handles_.end())
    {
      const std::string& type = controller.type;
      AllocatorsMap::iterator alloc_it = allocators_.find(type);
      if (alloc_it == allocators_.end())
      {  // create allocator is needed
        alloc_it = allocators_.insert(std::make_pair(type, loader_.createUniqueInstance(type))).first;
      }

      std::vector<std::string> resources;
      // Collect claimed resources across different hardware interfaces
      for (const auto& resource : controller.claimed_interfaces)
      {
        resources.push_back(parseJointNameFromResource(resource));
      }

      moveit_controller_manager::MoveItControllerHandlePtr handle =
          alloc_it->second->alloc(node_, name, resources);  // allocate handle
      if (handle)
        handles_.insert(std::make_pair(name, handle));
    }
  }

  /**
   * \brief get fully qualified name
   * @param name name to be resolved to an absolute name
   * @return resolved name
   */
  std::string getAbsName(const std::string& name)
  {
    return rclcpp::names::append(ns_, name);
  }

public:
  /**
   * \brief The default constructor. Reads the namespace from ~ros_control_namespace param and defaults to /
   */
  MoveItControllerManager()
    : loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator")
  {
    RCLCPP_INFO_STREAM(LOGGER, "Started moveit_ros_control_interface::MoveItControllerManager for namespace " << ns_);
  }

  /**
   * \brief Configure interface with namespace
   * @param ns namespace of ros_control node (without /controller_manager/)
   */
  MoveItControllerManager(const std::string& ns)
    : ns_(ns), loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator")
  {
  }

  void initialize(const rclcpp::Node::SharedPtr& node) override
  {
    node_ = node;
    if (!ns_.empty())
    {
      if (!node_->has_parameter("ros_control_namespace"))
        ns_ = node_->declare_parameter<std::string>("ros_control_namespace", "/");
    }
    list_controllers_service_ = node_->create_client<controller_manager_msgs::srv::ListControllers>(
        getAbsName("controller_manager/list_controllers"));
    switch_controller_service_ = node_->create_client<controller_manager_msgs::srv::SwitchController>(
        getAbsName("controller_manager/switch_controller"));
  }
  /**
   * \brief Find and return the pre-allocated handle for the given controller.
   * @param name
   * @return
   */
  moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override
  {
    std::unique_lock<std::mutex> lock(controllers_mutex_);
    HandleMap::iterator it = handles_.find(name);
    if (it != handles_.end())
    {  // controller is is manager by this interface
      return it->second;
    }
    return moveit_controller_manager::MoveItControllerHandlePtr();
  }

  /**
   * \brief Refresh controller list and output all managed controllers
   * @param[out] names list of controllers (with namespace)
   */
  void getControllersList(std::vector<std::string>& names) override
  {
    std::unique_lock<std::mutex> lock(controllers_mutex_);
    discover();

    for (std::pair<const std::string, controller_manager_msgs::msg::ControllerState>& managed_controller :
         managed_controllers_)
    {
      names.push_back(managed_controller.first);
    }
  }

  /**
   * \brief Refresh controller list and output all active, managed controllers
   * @param[out] names list of controllers (with namespace)
   */
  void getActiveControllers(std::vector<std::string>& names) override
  {
    std::unique_lock<std::mutex> lock(controllers_mutex_);
    discover();

    for (std::pair<const std::string, controller_manager_msgs::msg::ControllerState>& managed_controller :
         managed_controllers_)
    {
      if (isActive(managed_controller.second))
        names.push_back(managed_controller.first);
    }
  }

  /**
   * \brief Read resources from cached controller states
   * @param[in] name name of controller (with namespace)
   * @param[out] joints
   */
  void getControllerJoints(const std::string& name, std::vector<std::string>& joints) override
  {
    std::unique_lock<std::mutex> lock(controllers_mutex_);
    ControllersMap::iterator it = managed_controllers_.find(name);
    if (it != managed_controllers_.end())
    {
      for (const auto& claimed_resource : it->second.claimed_interfaces)
      {
        joints.push_back(claimed_resource);
      }
    }
  }

  /**
   * \brief Refresh controller state and output the state of the given one, only active_ will be set
   * @param[in] name name of controller (with namespace)
   * @return state
   */
  ControllerState getControllerState(const std::string& name) override
  {
    std::unique_lock<std::mutex> lock(controllers_mutex_);
    discover();

    ControllerState c;
    ControllersMap::iterator it = managed_controllers_.find(name);
    if (it != managed_controllers_.end())
    {
      c.active_ = isActive(it->second);
    }
    return c;
  }

  /**
   * \brief Filter lists for managed controller and computes switching set.
   * Stopped list might be extended by unsupported controllers that claim needed resources
   * @param activate
   * @param deactivate
   * @return true if switching succeeded
   */
  bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate) override
  {
    std::unique_lock<std::mutex> lock(controllers_mutex_);
    discover(true);

    typedef boost::bimap<std::string, std::string> resources_bimap;

    resources_bimap claimed_resources;

    // fill bimap with active controllers and their resources
    for (std::pair<const std::string, controller_manager_msgs::msg::ControllerState>& active_controller :
         active_controllers_)
    {
      for (const auto& resource : active_controller.second.claimed_interfaces)
      {
        claimed_resources.insert(resources_bimap::value_type(active_controller.second.name, resource));
      }
    }

    auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
    for (const std::string& it : deactivate)
    {
      ControllersMap::iterator c = managed_controllers_.find(it);
      if (c != managed_controllers_.end())
      {  // controller belongs to this manager
        request->stop_controllers.push_back(c->second.name);
        claimed_resources.right.erase(c->second.name);  // remove resources
      }
    }

    for (const std::string& it : activate)
    {
      ControllersMap::iterator c = managed_controllers_.find(it);
      if (c != managed_controllers_.end())
      {  // controller belongs to this manager
        request->start_controllers.push_back(c->second.name);
        for (const auto& claimed_resource : c->second.claimed_interfaces)
        {
          resources_bimap::right_const_iterator res = claimed_resources.right.find(claimed_resource);
          if (res != claimed_resources.right.end())
          {                                                    // resource is claimed
            request->stop_controllers.push_back(res->second);  // add claiming controller to stop list
            claimed_resources.left.erase(res->second);         // remove claimed resources
          }
        }
      }
    }
    request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;

    if (!request->start_controllers.empty() || !request->stop_controllers.empty())
    {  // something to switch?
      auto result_future = switch_controller_service_->async_send_request(request);
      if (result_future.wait_for(std::chrono::duration<double>(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout)
      {
        RCLCPP_ERROR_STREAM(LOGGER, "Couldn't switch controllers at " << switch_controller_service_->get_service_name()
                                                                      << " within " << SERVICE_CALL_TIMEOUT
                                                                      << " seconds");
        return false;
      }
      discover(true);
      return result_future.get()->ok;
    }
    return true;
  }
};
/**
 *  \brief MoveItMultiControllerManager discovers all running ros_control node and delegates member function to the
 * corresponding MoveItControllerManager instances
 */
class MoveItMultiControllerManager : public moveit_controller_manager::MoveItControllerManager
{
  typedef std::map<std::string, moveit_ros_control_interface::MoveItControllerManagerPtr> ControllerManagersMap;
  ControllerManagersMap controller_managers_;
  rclcpp::Time controller_managers_stamp_{ 0, 0, RCL_ROS_TIME };
  std::mutex controller_managers_mutex_;

  rclcpp::Node::SharedPtr node_;

  void initialize(const rclcpp::Node::SharedPtr& node) override
  {
    node_ = node;
  }
  /**
   * \brief  Poll ROS master for services and filters all controller_manager/list_controllers instances
   * Throttled down to 1 Hz, controller_managers_mutex_ must be locked externally
   */
  void discover()
  {
    // Skip if last discovery is too new for discovery rate
    if ((node_->now() - controller_managers_stamp_) < CONTROLLER_INFORMATION_VALIDITY_AGE)
      return;

    controller_managers_stamp_ = node_->now();

    const std::map<std::string, std::vector<std::string>> services = node_->get_service_names_and_types();

    for (const auto& service : services)
    {
      const auto& service_name = service.first;
      std::size_t found = service_name.find("controller_manager/list_controllers");
      if (found != std::string::npos)
      {
        std::string ns = service_name.substr(0, found);
        if (controller_managers_.find(ns) == controller_managers_.end())
        {  // create MoveItControllerManager if it does not exists
          RCLCPP_INFO_STREAM(LOGGER, "Adding controller_manager interface for node at namespace " << ns);
          auto controller_manager = std::make_shared<moveit_ros_control_interface::MoveItControllerManager>(ns);
          controller_manager->initialize(node_);
          controller_managers_.insert(std::make_pair(ns, controller_manager));
        }
      }
    }
  }

  /**
   * \brief Get namespace (including leading and trailing slashes) from controller name
   * @param name
   * @return extracted namespace or / if none is found
   */
  static std::string getNamespace(const std::string& name)
  {
    size_t pos = name.find('/', 1);
    if (pos == std::string::npos)
      pos = 0;
    return name.substr(0, pos + 1);
  }

public:
  /**
   * \brief Find appropriate interface and delegate handle creation
   * @param name
   * @return handle
   */
  moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override
  {
    std::unique_lock<std::mutex> lock(controller_managers_mutex_);

    std::string ns = getNamespace(name);
    ControllerManagersMap::iterator it = controller_managers_.find(ns);
    if (it != controller_managers_.end())
    {
      return it->second->getControllerHandle(name);
    }
    return moveit_controller_manager::MoveItControllerHandlePtr();
  }

  /**
   * \brief Read all managed controllers from discovered interfaces
   * @param names
   */
  void getControllersList(std::vector<std::string>& names) override
  {
    std::unique_lock<std::mutex> lock(controller_managers_mutex_);
    discover();

    for (std::pair<const std::string, moveit_ros_control_interface::MoveItControllerManagerPtr>& controller_manager :
         controller_managers_)
    {
      controller_manager.second->getControllersList(names);
    }
  }

  /**
   * \brief Read all active, managed controllers from discovered interfaces
   * @param names
   */
  void getActiveControllers(std::vector<std::string>& names) override
  {
    std::unique_lock<std::mutex> lock(controller_managers_mutex_);
    discover();

    for (std::pair<const std::string, moveit_ros_control_interface::MoveItControllerManagerPtr>& controller_manager :
         controller_managers_)
    {
      controller_manager.second->getActiveControllers(names);
    }
  }

  /**
   * \brief Find appropriate interface and delegate joints query
   * @param name
   * @param joints
   */
  void getControllerJoints(const std::string& name, std::vector<std::string>& joints) override
  {
    std::unique_lock<std::mutex> lock(controller_managers_mutex_);

    std::string ns = getNamespace(name);
    ControllerManagersMap::iterator it = controller_managers_.find(ns);
    if (it != controller_managers_.end())
    {
      it->second->getControllerJoints(name, joints);
    }
  }

  /**
   * \brief Find appropriate interface and delegate state query
   * @param name
   * @return
   */
  ControllerState getControllerState(const std::string& name) override
  {
    std::unique_lock<std::mutex> lock(controller_managers_mutex_);

    std::string ns = getNamespace(name);
    ControllerManagersMap::iterator it = controller_managers_.find(ns);
    if (it != controller_managers_.end())
    {
      return it->second->getControllerState(name);
    }
    return ControllerState();
  }

  /**
   * \brief delegates switch  to all known interfaces. Stops on first failing switch.
   * @param activate
   * @param deactivate
   * @return
   */
  bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate) override
  {
    std::unique_lock<std::mutex> lock(controller_managers_mutex_);

    for (std::pair<const std::string, moveit_ros_control_interface::MoveItControllerManagerPtr>& controller_manager :
         controller_managers_)
    {
      if (!controller_manager.second->switchControllers(activate, deactivate))
        return false;
    }
    return true;
  }
};

}  // namespace moveit_ros_control_interface

PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::MoveItControllerManager,
                       moveit_controller_manager::MoveItControllerManager);

PLUGINLIB_EXPORT_CLASS(moveit_ros_control_interface::MoveItMultiControllerManager,
                       moveit_controller_manager::MoveItControllerManager);