mission_planning_client.cpp

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2015, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 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 the University of Aveiro 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 HOLDER 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.
***************************************************************************************************/

/*
@file
@brief
This is the client file, it will establish a connection with the gps receiver , a database and comunicate with the mission_planning_node
*/
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>

#include<mission_planning/Path.h>

#include <vector>
#include <math.h>

#include <postgresql/libpq-fe.h>
#include <string>

#include <utility> //std::pair

#define d2r (M_PI / 180.0) //degree to radians

/*
@brief
The following object is the pointer that will handle a connection to a database
@param user
This is the user that has access to the database
@param password
Password required to establish a connection to the database
@param dbname
Name of the database that hold our tables
@param hostaddr
Ip of our database server, in this case because it is on our computer and launched as localhost, it will be 127.0.0.1
@param port
The port to connect to the database
*/
//PGconn *conn = PQconnectdb("user=ivan password=ivanferreira dbname=ivan hostaddr=127.0.0.1 port=5432");
PGconn *conn = PQconnectdb("user=atlas password=atlascar dbname=atlas_navigation hostaddr=127.0.0.1 port=5432");

/*
@brief
Function to disconnect from the database
@param conn 
connection handle
@return void
*/
/* Close connection to database */
void CloseConn(PGconn *conn)
{
    PQfinish(conn);
    printf("Database Disconnected - OK\n");
    //getchar(); //waits for user input
    exit(1); //terminates program -- remove later
}
/*
@brief
Establishes a connection to a database, in case success or fail a message appears in the console
@return conn
*/
/* Establish connection to database */
PGconn *ConnectDB()
{
    PGconn *conn = NULL;

    // Make a connection to the database
    //conn = PQconnectdb("user=ivan password=ivanferreira dbname=ivan hostaddr=127.0.0.1 port=5432");
    conn = PQconnectdb("user=atlas password=atlascar dbname=atlas_navigation hostaddr=127.0.0.1 port=5432");

    // Check to see that the backend connection was successfully made
    if (PQstatus(conn) != CONNECTION_OK)
    {
        printf("Connection to database failed");
        CloseConn(conn);
    }

    printf("Connection to database - OK\n");

    return conn;
}

void DBConnectionCheck(PGconn *conn){

    // Check to see that the backend connection was successfully made
    if (PQstatus(conn) != CONNECTION_OK)
    {
        printf("Connection to database failed");
        CloseConn(conn);
    }

    printf("Connection to database - OK\n");
}
/*
@brief
An example function. Is not used.
*/
/* Append SQL statement and insert record into specific table, NOT used, serves as an example */
void InsertEmployeeRec(PGconn *conn, double fname)//void InsertEmployeeRec(PGconn *conn, char * fname, char * lname)
{
    // Append the SQL statment
    /* std::string sSQL;
  sSQL.append("INSERT INTO closest_waypoint_table VALUES ('");
  sSQL.append(fname);
  sSQL.append("')");*/
    // std::string sSQL = "INSERT INTO closest_waypoint_table (closest_index) VALUES ('"+ std::to_string(fname) + "')";
    std::string sSQL = "INSERT INTO random (rand_stuff) VALUES ("+ std::to_string(fname) + ")";
    // Execute with sql statement
    PGresult *res = PQexec(conn, sSQL.c_str());

    if (PQresultStatus(res) != PGRES_COMMAND_OK)
    {
        printf("Insert employee record failed");
        PQclear(res);
        CloseConn(conn);
    }

    printf("Insert employee record - OK\n");

    // Clear result
    PQclear(res);
}

/* Erase all record in specific table, NOT used, serves as an example */
void RemoveAllEmployeeRec(PGconn *conn)
{
    // Execute with sql statement
    PGresult *res = PQexec(conn, "DELETE FROM random");

    if (PQresultStatus(res) != PGRES_COMMAND_OK)
    {
        printf("Delete employees record failed.");
        PQclear(res);
        CloseConn(conn);
    }

    printf("\nDelete employees record - OK\n");

    // Clear result
    PQclear(res);
}
/*
@brief
Function to insert the gps coordinates into the table named gps_coord_table
@param conn
Connection handler
@param sensor_msgs::NavSatFixPtr
Contains the coordinates from the gps
@return void
*/
// Insert gps coordinates into table
void  InsertGpsCoordRec(PGconn *conn, const sensor_msgs::NavSatFixPtr &gps)
{
    std::string sSQL = "INSERT INTO gps_coord_table (gps_lat,gps_lon) VALUES ("+ std::to_string(gps->latitude) + "," +std::to_string(gps->longitude)+ ")";

    PGresult *res = PQexec(conn, sSQL.c_str());

    if (PQresultStatus(res) != PGRES_COMMAND_OK)
    {
        printf("Insert gps coordinates record failed");
        PQclear(res);
        CloseConn(conn);
    }

    printf("Insert gps coordinates record - OK\n");

    // Clear result
    PQclear(res);
}
/*
@brief
Function that inserts the waypoints into the table named waypoint_table
@param conn
connection handler
@param mission_planning::Path::Response
contains the waypoints and instructions sent by the mission_planning_node
@return void
*/
// Insert Waypoint into table //NEEDS REWORK INCOMPATIBLE TYPES

void  InsertWaypointsRec(PGconn *conn, const mission_planning::Path::Response &resp)
{
    for (int i = 0; i < resp.lat.size(); ++i)
    {
        double glat = resp.lat[i];
        double glon = resp.lon[i];
        std::string ginst = resp.instruction[i];
        // std::string sSQL = "INSERT INTO waypoint_table (way_lat,way_lon,instruction) VALUES ("+ std::to_string(resp.lat[i]) + "," + std::to_string(resp.lon[i])+ "," + std::to_string(resp.instruction[i]) + ")";
        std::string sSQL = "INSERT INTO waypoint_table (way_lat,way_lon,instruction) VALUES ("+ std::to_string(glat) + "," + std::to_string(glon)+ ",'" + ginst + "')";

        PGresult *res = PQexec(conn, sSQL.c_str());

        if (PQresultStatus(res) != PGRES_COMMAND_OK)
        {
            printf("Insert waypoints record failed: %s", PQerrorMessage(conn));
            PQclear(res);
            CloseConn(conn);
        }

        printf("Insert waypoints record - OK\n");

        // Clear result
        PQclear(res);
    }

}
/*
@brief
Insert the closest waypoint into the table named closest_waypoint_table
@param conn
connection handler
@param closestwaypoint
contains the closest waypoint
@return void
*/
//Insert Closest Waypoint into table
void  InsertClosestWaypointRec(PGconn *conn, int closestwaypoint)
{
    std::string sSQL = "INSERT INTO closest_waypoint_table (closest_index) VALUES ("+ std::to_string(closestwaypoint) +")";

    PGresult *res = PQexec(conn, sSQL.c_str());

    if (PQresultStatus(res) != PGRES_COMMAND_OK)
    {
        printf("Insert Closest Waypoint record failed");
        PQclear(res);
        CloseConn(conn);
    }

    printf("Insert Closest Waypoint - OK\n");

    // Clear result
    PQclear(res);
}
/*
@brief
Function that inserts the next instruction into the table named next_inst_table
@param conn
connection handler
@param mission_planning::Path::Response
contains the waypoints and instructions sent by the mission_planning_node
@param nextinstruction
contains the next instruction that needs to be executed
@return void
*/
//Insert Next Instruction into table BETA
void InsertNextInstructionRec (PGconn *conn, const mission_planning::Path::Response &resp, int nextinstruction)
{
     std::string ginst = resp.instruction[nextinstruction];
     std::string sSQL = "INSERT INTO next_inst_table (next_inst) VALUES ('" + ginst + "')";
     PGresult *res = PQexec(conn, sSQL.c_str());

     if (PQresultStatus(res) != PGRES_COMMAND_OK)
     {
         printf("Insert Next Instruction record failed");
         PQclear(res);
         CloseConn(conn);
     }

     printf("Insert Next Instruction - OK\n");

     // Clear result
     PQclear(res);
}
/*
@brief
This function deletes all values from all tables
@param conn
connection handler
@return void
*/
/* Erase all record in all tables */
void RemoveAllTablesRec(PGconn *conn)
{
    //--------------------- Table I  ---------------
    // Execute with sql statement
    PGresult *res1 = PQexec(conn, "DELETE FROM closest_waypoint_table");

    if (PQresultStatus(res1) != PGRES_COMMAND_OK)
    {
        printf("Delete closest waypoint record failed.");
        PQclear(res1);
        CloseConn(conn);
    }

    printf("\nDelete closest waypoint record - OK\n");

    // Clear result
    PQclear(res1);

    //--------------------- Table II  ---------------
    // Execute with sql statement
    PGresult *res2 = PQexec(conn, "DELETE FROM waypoint_table");

    if (PQresultStatus(res2) != PGRES_COMMAND_OK)
    {
        printf("Delete waypoint table record failed.");
        PQclear(res2);
        CloseConn(conn);
    }

    printf("\nDelete waypoint table record - OK\n");

    // Clear result
    PQclear(res2);

    //--------------------- Table III ---------------
    // Execute with sql statement
    PGresult *res3 = PQexec(conn, "DELETE FROM gps_coord_table");

    if (PQresultStatus(res3) != PGRES_COMMAND_OK)
    {
        printf("Delete gps coordinates record failed.");
        PQclear(res3);
        CloseConn(conn);
    }

    printf("\nDelete gps coordinates record - OK\n");

    // Clear result
    PQclear(res3);

    //--------------------- Table IV ---------------
    // Execute with sql statement
    PGresult *res4 = PQexec(conn, "DELETE FROM next_inst_table");

    if (PQresultStatus(res4) != PGRES_COMMAND_OK)
    {
        printf("Delete next instruction record failed.");
        PQclear(res4);
        CloseConn(conn);
    }

    printf("\nDelete next instruction record - OK\n");

    // Clear result
    PQclear(res4);
}
/*
@brief
Example function to fetch a result from a database. This function is not beeing used.
*/
/* Fetch destination record and return a vector containing the data */
std::vector<std::string> FetchEmployeeRec(PGconn *conn){

    PGresult *res = PQexec(conn,"SELECT * FROM destination_table ORDER BY dest_id DESC LIMIT 1;");
    int rec_count;

    if (PQresultStatus(res) != PGRES_TUPLES_OK) {
        printf("No data in table, make sure you submited a valid destination from the Webpage ! Try again");
        PQclear(res);
        CloseConn(conn);
    }

    rec_count = PQntuples(res);

    printf("Obtained %d records.\n", rec_count);

    std::vector<std::string> destRecord;

    for (int row=0; row<rec_count; row++) {

        //printf("%s\t", PQgetvalue(res, row, col));
        destRecord.push_back(PQgetvalue(res, row, 1));  //dest_lat
        destRecord.push_back(PQgetvalue(res, row, 2));  //dest_lon
        std::cout << destRecord[0] << std::endl;
        std::cout << destRecord[1] << std::endl;
    }

    PQclear(res);
    return destRecord;

}
/*
@brief
Function that calculates the haversine distance between two gps coordinates.
Returns the distance calculated in km.
@param lat1
actual latitude
@param lon1
actual longitude
@param lat2
next latitude
@param lon2
next longitude
@return d Distance in kilometers
*/
//calculate haversine distance for linear distance (distance between two gps coordinates)
double haversine_km(double lat1, double lon1, double lat2, double lon2)
{
    double dlon = (lon2 - lon1) * d2r;
    double dlat = (lat2 - lat1) * d2r;
    double a = pow(sin(dlat/2.0), 2) + cos(lat1*d2r) * cos(lat2*d2r) * pow(sin(dlon/2.0), 2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = 6371 * c; //mean earth radius 6371KM

    return d; //returns distance in km
}
/*
@brief 
Function that calculates the shortest distance between the current position and all waypoints.
Returns the index of the shortest distance in the waypoints array.
@param sensor_msgs::NavSatFixPtr
contains the current gps coordinates
@param mission_planning::Path::Response
contains the waypoints and instructions sent by the mission_planning_node
@return min_index index of the shortest distance in the waypoints array
*/
//euclidian Distance function
double shortestDistance(const sensor_msgs::NavSatFixPtr &gps,const mission_planning::Path::Response& resp){

    //new vector to hold all calculated distances
    std::vector<double> allDists;
    //clear all elements in vector allDists ! Guess its not required ... the destructor does the job .
    allDists.clear();

    //loop through all elements in the response and calculate the minimum distance (euclidian distance) between the
    // actual gps point and all waypoints. store the distances in the allDist vector
    for (int i = 0; i < resp.lat.size(); ++i) {
        /*
        double x = resp.lat.at(i) - gps->latitude;
        double y = resp.lon.at(i) - gps->longitude;
        //  double x = resp.lat.at(i) - 40.634062;//40.63179
        //  double y = resp.lon.at(i) - -8.657999;//-8.65712
        double dist = pow(x,2) + pow(y,2);
        dist = sqrt(dist);
        */
        double dist = haversine_km(resp.lat.at(i),resp.lon.at(i),gps->latitude,gps->longitude);
        allDists.push_back(dist);
    }

    //get the index of the smallest element in the allDist vector
    int min_index = std::min_element(allDists.begin(), allDists.end()) - allDists.begin();
    /*
    //recommended usage
    int indexOfSmallestElement = distance(vec.begin(),min_element(vec.begin(),vec.end()));
    //returns the smallest element found in the allDist vector
    return allDists.at(min_index);//checks for out_of_bounds
    */
    //std::cout << "Closest waypoint to the current gps location is: " << resp.lat[min_index] << resp.lon[min_index] << std::endl;
    //return allDists[min_index];//faster access

    return min_index;//return only min_index !!!! access out of this function to the resp.lat[min_index]

}
/*
@brief
Function that checks if the car went out of route.
If the car goes away from the next two waypoints and the distance increases and passes the 75m, this function will trigger.
@param sensor_msgs::NavSatFixPtr
contains the current gps coordinates
@param mission_planning::Path::Response
contains the waypoints and instructions sent by the mission_planning_node
@return current instruction or next instruction
*/
//checks if from the closest waypoint to 75 further if there is any instruction BETA
double nextInst(const sensor_msgs::NavSatFixPtr &gps,const mission_planning::Path::Response& resp){
    //new vector to hold all calculated distances
    std::vector<double> allDists;
    //clear all elements in vector allDists ! Guess its not required ... the destructor does the job .
    allDists.clear();

    //loop through all elements in the response and calculate the minimum distance (euclidian distance) between the
    // actual gps point and all waypoints. store the distances in the allDist vector
    for (int i = 0; i < resp.lat.size(); ++i) {

        double dist = haversine_km(resp.lat.at(i),resp.lon.at(i),gps->latitude,gps->longitude);

        allDists.push_back(dist); //fazer push back apenas se a distancia for inferior ou igual a 75m !

    }

    //get the index of the smallest element in the allDist vector
    int min_index = std::min_element(allDists.begin(), allDists.end()) - allDists.begin();


    std::vector<double> smallerthan (allDists.size()); //will hold all distances shorter than 75m

    // copy only numbers smaller than 75meters into smallerthan vector , starting at the next waypoint after the smallest distance
    auto it = std::copy_if (allDists.begin()+min_index+1, allDists.end(), smallerthan.begin(), [](int i){return (i<=0.075);} );
    smallerthan.resize(std::distance(smallerthan.begin(),it));  // shrink container to new size
    //vector smallerthan contains now the distances shorter than 75meters

    std::vector<int> posis; //will contain the index of the instructions under 75meter

    for(int i=0; i<smallerthan.size(); ++i)
    {
        auto it2=find(allDists.begin(),allDists.end(),smallerthan[i]); //gets the index in foo of the distances in bar
        //it++;
        int pos = distance(allDists.begin(), it2);
        //std::cout <<"position index in foo " <<pos << "\n";
        posis.push_back(pos);
    }

    //WHAT IF THERE IS NO VALID INSTRUCTION ?, WHAT IF ALL ARE EMPTY ? WHAT DO I RETURN ?
    std::vector<int> validinstr; //will hold any valid instructions , not empty ones

    if(posis.size() > 0){ //make sure there is any instruction ( any distance under 75m)
        for (int i=0;i<posis.size();i++) { //scann all instructions
            if(!resp.instruction[posis[i]].empty()) //if string in instructions is not empty return it
            {
                std::cout << "Return this instruction, or better, return the index that contains the instruction" << "\n";
                validinstr.push_back(posis[i]);
            }
        }
    }
    if (validinstr.size() > 0) //if the vector contains any instruction
    {
        int valid_inst = validinstr[0];
        return valid_inst; //this is the index that contains the first next valid instruction
    }
    else
    {
        return min_index; //case there are no instructions return the current instruction
    }

}
/*
@brief
Function that calculates the distance between the current and the next waypoint
@param sensor_msgs::NavSatFixPtr
contains the current gps coordinates
@param mission_planning::Path::Response
contains the waypoints and instructions sent by the mission_planning_node
@param minimumIndex
current closest waypoint
@return pair of the closest distance and next distance
*/
std::pair<double,double> wrongDirection (const sensor_msgs::NavSatFixPtr &gps, const mission_planning::Path::Response &resp, int minimumIndex){

    double distance1 = 0;
    double distance2 = 0;

    if(minimumIndex+1 <= resp.lat.size()){

        distance1 = haversine_km(resp.lat.at(minimumIndex),resp.lon.at(minimumIndex),gps->latitude,gps->longitude); //ponto actual mais proximo
        distance2 = haversine_km(resp.lat.at(minimumIndex+1),resp.lon.at(minimumIndex+1),gps->latitude,gps->longitude); //ponto seguinte mais proximo
    }

    return std::make_pair(distance1,distance2);
    //return std::make_pair(-1,-1); ou 0,0 !!!
}

/*
bool wrongDirection (const sensor_msgs::NavSatFixPtr &gps, const mission_planning::Path::Response &resp){
    //new vector to hold all calculated distances
    std::vector<double> allDistsNow;
    std::vector<double> allDistsNext;
    //loop through all elements in the response and calculate the minimum distance (euclidian distance) between the
    // actual gps point and all waypoints. store the distances in the allDist vector
    for (int i = 0; i < resp.lat.size()-1; ++i) {

        double distance1 = haversine_km(resp.lat.at(i),resp.lon.at(i),gps->latitude,gps->longitude); //ponto actual mais proximo
        double distance2 = haversine_km(resp.lat.at(i+1),resp.lon.at(i+1),gps->latitude,gps->longitude); //ponto seguinte mais proximo

        allDistsNow.push_back(distance1);
        allDistsNext.push_back(distance2);

        if (allDistsNow.size()>2 && allDistsNext.size()>2)
        {
            //check if moving in wrong direction
            for (int j = 0; j < allDistsNow.size()-1; ++j) {
                if (allDistsNow[j+1] > allDistsNow[j] && allDistsNext[j+1] > allDistsNext[j])
                {
                    //Need to check for a specific distance or wait some time before recalculating
                   // if(allDistsNow[j+1]>=0.1 && allDistsNext[j+1]>=0.1); //distance greater than 100M
                    if(allDistsNow[j]>=0.1 && allDistsNext[j]>=0.1); //distance greater than 100M
                    std::cout << "Moving into wrong direction...Recalculating path..." << std::endl;
                    return true;
                }
                else
                {
                    return false;
                }
            }
        }
    }
}
*/
/*
@brief
Class that handles the connection with the gps (subscribes to the topic where the gps publishes data).
Everytime a new coordinate arrives from the gps, the gpscallback function is called and the code inside it is executed.
Contains the console menu, that accepts a user input.
*/
class GpsTest
{
public:

    // Constructor
    GpsTest(ros::NodeHandle nh_) : n(nh_) //n vai ser uma copia do nodehandle da class gpsTest
    {
        hasData = false; //checks if response contains data
        client = n.serviceClient<mission_planning::Path>("return_path"); //initialize client
        // Subscribing to the topic /fix (novatel lib -> topic /gps_fix)
        gps_sub = n.subscribe("/gps_fix_psr", 100, &GpsTest::gpsCallback, this);//vai ser criada uma nova instacia gps_sub, que subscreve o topico /gps_fix_psr  e chama a função gpsCallback
    }

    // Callback Function for the GPS
    void gpsCallback(const sensor_msgs::NavSatFixPtr &msg)
    {
        if(!hasData){
            //Request class
            mission_planning::Path::Request req;

            //ASK THE USER WHERE HE WANTS TO GO
            std::cout << "Choose locations or insert coordinates Lat/Lon " << std::endl;
            std::cout << "Option 1: DEM " << std::endl;
            std::cout << "Option 2: IT " << std::endl;
            std::cout << "Option 3: CRASTO " << std::endl;
            std::cout << "Option 4: REITORIA " << std::endl;
            std::cout << "Option 5: GARAGEM " << std::endl;
            std::cout << "Option 6: PRAIA DA BARRA " << std::endl;
            std::cout << "Option 7: CUSTOM DESTINATION (ex: 40.629418 ; -8.658260)" << std::endl;
            std::cout << "Option 8: DESTINATION FROM WEBPAGE, SELECT A VALID DESTINATION FROM THE WEBPAGE FIRST THEN PRESS 8 " << std::endl;
            int anwser; int customLat; int customLon;
            std::cin >> anwser ;
            switch (anwser) {
            case 1:
                req.destLat = 40.629418;
                req.destLon = -8.658260;
                break;
            case 2:
                req.destLat = 40.643282;
                req.destLon = -8.659614;
                break;
            case 3:
                req.destLat = 40.623604;
                req.destLon = -8.657467;
                break;
            case 4:
                req.destLat = 40.631689;
                req.destLon = -8.657190;
                break;
            case 5:
                req.destLat = 40.634114;
                req.destLon = -8.657181;
                break;
            case 6:
                req.destLat = 40.633250;
                req.destLon = -8.745934;
                break;
            case 7:
                std::cin >> customLat ;
                std::cout << "\n" ;
                std::cin >> customLon ;
                req.destLat = customLat;//user input
                req.destLon = customLon;//user input
                break;
            case 8:
                //coords invalidas 40.637885, -8.655428
                std::string::size_type sz;     // alias of size_t
                req.destLat = std::stod (FetchEmployeeRec(conn)[0],&sz); //converts string to double
                req.destLon = std::stod (FetchEmployeeRec(conn)[1],&sz);
                break;

            default: //Garagem
                req.destLat = 40.635114;
                req.destLon = -8.656467;
                break;
            }

            //if the coordinates are diferent than lat=0.0000 and lon=0.0000 allow to process data else print
            // "Waiting for GPS fix!"
            req.gpsLat = msg->latitude;
            req.gpsLon = msg->longitude;

            //while(req.gpsLat == 0 && req.gpsLon == 0){ printf("Waiting for gps fix ..."); }


            //if the request to the server is successfull
            if(client.call(req,resp)){
                hasData = true;

                InsertWaypointsRec(conn,resp); //Placed here because it should only write to this database when a new route is generated
            }//if client call
        }//if(!hasData)

        //calls processData function
        if(hasData)
            processData(msg,resp);

        //std::cout << "Current Latitude: " << msg->longitude << std::endl;
        //std::cout << "Current Longitude " << msg->latitude << std::endl;
        /*
        std::cout << "Lat Response Size" << resp.lat.size() <<std::endl;
        std::cout << "Lon Response Size" << resp.lon.size() <<std::endl;
        std::cout << "Instruction Response Size" << resp.instruction.size() <<std::endl;
        */

        //shortestDistance(msg,resp);
        double minIndex = shortestDistance(msg,resp);
        // std::cout << "Closest waypoint to the current gps location is: " << resp.lat[minIndex] <<" "<<resp.lon[minIndex] << std::endl;
        printf("Closest waypoint to the current gps location is: %10.6f %10.6f \n",resp.lat[minIndex],resp.lon[minIndex]);
        std::cout << "Instruction TODO: " << resp.instruction[minIndex] << std::endl;


        //next instruction BETA
        double nextInstruction = nextInst(msg,resp);
        std::cout << "Upcoming Instruction: " << resp.instruction[nextInstruction] << std::endl;

        //store the response and compare with the new one, if it is equal dont show it
        //com o indice ir ao vector da resposta e apresentar a instrução a realizar

        //Writes to DB
        InsertGpsCoordRec(conn, msg); //ATENÇÂO ESTE conn pode nao funcionar pq nao foi declarado aqui !!!!
        InsertClosestWaypointRec(conn, minIndex);
        InsertNextInstructionRec(conn, resp, nextInstruction); //BETA

        static int sentinel = 0;
        static double closeDist1;
        static double closeDist2;
        //std::pair<double, double> p =  wrongDirection(msg,resp,minIndex);
        if (sentinel == 0) {
            std::pair<double, double> p =  wrongDirection(msg,resp,minIndex);
            closeDist1 = p.first; //Distance to closest waypoint
            closeDist2 = p.second; //Distance to closest waypoint + 1
            sentinel = 1 ;
        }else if (sentinel == 1) {
            if(closeDist1 !=0 && closeDist2 !=0){
                std::pair<double, double> p2 =  wrongDirection(msg,resp,minIndex);
                double closeDist12 = p2.first;  //Distance to closest waypoint, new check
                double closeDist22= p2.second;

                //Check if the distance to the last check has increased, if "YES" then check if it is greater than 75m (100m = 0.1)
                if((closeDist12 > closeDist1) && closeDist12 >= 0.075 && (closeDist22 > closeDist2) && closeDist22 >= 0.075)

                {
                    //Recalculate route
                    mission_planning::Path::Request req;

                    req.destLat = resp.lat.back(); //last latitude element in vector ( previous destination )
                    req.destLon = resp.lon.back();

                    //current position
                    req.gpsLat = msg->latitude;
                    req.gpsLon =  msg->longitude;

                    //Do a new request to the server
                    if(client.call(req,resp)){
                        hasData = true;
                        printf("Recalculating Route...");
                    }
                    //fazer a limpeza das bases de dados, porque é gerada uma nova rota !
                    RemoveAllTablesRec(conn);
                    InsertWaypointsRec(conn,resp); //Placed here because it should only write to this database when a new route is generated
                }
            }
            sentinel = 0 ; //Reset static variable
        }


        /*
        //NEEDS to be tested !
        if(wrongDirection(msg,resp))
        {
            //Request class
            mission_planning::Path::Request req;

            req.destLat = resp.lat.back(); //last latitude element in vector ( previous destination )
            req.destLon = resp.lon.back();
            req.gpsLat = msg->latitude;
            req.gpsLon =  msg->longitude;

            if(client.call(req,resp)){
                hasData = true;
            }
            //fazer a limpeza das bases de dados, porque é gerada uma nova rota !
            RemoveAllTablesRec(conn);
        }
*/
    } //end void gpscallback

    //Able to process data
    bool processData(const sensor_msgs::NavSatFixPtr &gps,const mission_planning::Path::Response& resp){

        //gps->latitude;
        //gps->longitude;

        //std::cout<<"Processar a nova posição de gps, usando o que estiver em resp, já lá estão dados"<<std::endl;
        //std::cout << resp.lat.size() << std::endl;
        /*
        for (int i = 0; i < resp.lat.size(); ++i) {
            std::cout << resp.lat.at(i) << std::endl;

        }
        */

        /*
        for(std::vector<double>::iterator itDouble = resp.lat.begin(); itDouble != resp.lat.end(); itDouble++){
            std::cout << *itDouble << std::endl;
        }
        */

        return true;
    }

    
private:
    // Nodehandle
    ros::NodeHandle n;
    //Response class
    mission_planning::Path::Response resp;

    // Subscriber
    ros::Subscriber gps_sub;
    ros::ServiceClient client;
    bool firstData;
    bool hasData;
};

/*
@brief
Main of the program.
Responsible to establish a connection to the database, clear all tables, initialize the node, the constructor and at the end close the connection with the database.
*/
int main(int argc, char** argv)
{

    /*
       * Make sure that you have followed the steps bellow, in case something goes wrong !
       * first: roscore
       * second: roslaunch novatel novatel_for_psr.launch (Make sure that the gps is powered on and connected to the USB port before launching this !!!)
       * third: rosrun mission_planning mission_planning_client  (Make sure that OSRM-backend is up and running !!! )
       * forth: rosrun mission_planning mission_planning_node
      */
    /*
      PGconn *conn = NULL;
      conn = ConnectDB(); //connects to DB
*/
    DBConnectionCheck(conn);
    RemoveAllTablesRec(conn); //Clean all tables
    //InsertEmployeeRec(conn, -7.1312312414); // o max que da para colocar na table é 6 casas decimais
    // InsertEmployeeRec(conn, 180.1712912454);
    //FetchEmployeeRec(conn);
    //RemoveAllEmployeeRec(conn);

    // Initializing the node for the GPS
    ros::init(argc, argv, "gps_Subscriber"); //novo no para comunicação
    ros::NodeHandle nh_;


    GpsTest p(nh_); //inicialização do construtor
    ros::spin(); // necessario para ler dados do topico mas não para pedir coisas ao servidor

    CloseConn(conn); //Disconnects from Database, is executed after the program is terminated
    //PQfinish(PQconnectdb("user=ivan password=ivanferreira dbname=ivan hostaddr=127.0.0.1 port=5432"));
    //PQfinish(PQconnectdb("user=atlas password=atlascar dbname=atlas_navigation hostaddr=127.0.0.1 port=5432"));
    //CloseConn(PQconnectdb("user=ivan password=ivanferreira dbname=ivan hostaddr=127.0.0.1 port=5432"));
    return 0;

}