mission_planning_node.cpp

Go to the documentation of this file.

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

 Copyright (c) 2011-2013, 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
Responsible to comunicate with the osrm-backend.
Receives the destination and returns the waypoints and instructions.
*/
#include <iostream>
#include <stdio.h>

#include <algorithm> // for std::find
#include <iterator> // for std::begin, std::end

//INCLUDE CURL
#include <curl/curl.h>
#include <boost/asio.hpp>

//INCLUDE JSON
#include "json/json.h"
#include <string>

//INCLUDE GOOGLEPOLYLINE
#include <vector>
#include <math.h>
#include <bitset>
#include <utility>

//ROS INCLUDES
#include <ros/ros.h>
#include <mission_planning/Path.h>

using namespace std;
//CLASSES
/*
@brief
Encapsulate gps coordiantes and maneuver.
Creates a class of informations relative to the mission. Can be exapanded further.
*/
class GpsLocation{

public:

    typedef boost::shared_ptr<GpsLocation> Ptr;

    double latitude;
    double longitude;
    //double altitude;
    string maneuver;

    GpsLocation(double latitude,double longitude,string maneuver){
        this->latitude = latitude;
        this->longitude = longitude;
        this->maneuver = maneuver;
    }
};
/*
@brief
Operator << overload
Overload for opeartor << to print directly all elements of the locations vector
*/
//Overload for opeartor << to print directly all elements of locations vector
ostream& operator<<(ostream& o, const GpsLocation::Ptr& loc){
    if(loc->maneuver.empty())
        o<<"Lat:" <<loc->latitude<<" Lon: "<<loc->longitude<<" Man: (none)";
    else
        o<<"Lat:" <<loc->latitude<<" Lon: "<<loc->longitude<<" Man: "<<loc->maneuver;

    return o;
}
/*
@brief
Function to decode the encoded polyline
This function decodes the polyline from the osrm-backend.
@param encodedPoints
The polyline string
@param precision
The precision of the encoded polyline, in this case it is six digits.
@return
Array containing the latitude and longitude values.
*/
//FUNCTION TO DECODE ENCODED POLYLINE
vector<double>Decompress(string encodedPoints, int precision){

    int len = encodedPoints.length();
    int index =0;
    double lat =0;
    double lng = 0;
    vector<double> array;

    while (index < len) {

        int b;
        int shift =0;
        int result =0;
        do {
            b = (int)encodedPoints[index++] - 63; //gets ascii value of the char
            //result |= (b & 0x1f) << shift;
            result |= (b & 31) << shift;
            shift +=5;
        } while (b>=32);
        int dlat= ((result & 1) ? ~(result >> 1) : (result >> 1));
        lat+=dlat;
        shift =0;
        result =0;

        do {
            b = (int)encodedPoints[index++]- 63; //gets ascii value of the char
            result |= (b & 31) << shift;
            shift +=5;
        } while (b>=32);
        int dlng = ((result & 1) ? ~(result >> 1) : (result >> 1));
        lng+=dlng;

        array.push_back(lat*pow(10,-precision));
        array.push_back(lng*pow(10,-precision));
    }
    return array;
}
/*
@brief
Writes curl repsonse to string format
*/
//WRITE CURL RESPONSE TO STRING FORMAT
size_t write_to_string(void* ptr, size_t size, size_t nmemb, std::string* data){
    data->append((const char*)ptr, size * nmemb);
    return size * nmemb;
}

/*
@brief
Function that requests a route
This functions decodes the information sent by the mission_planning_client module and creates a string that will be sent over curl to the osrm-backend located in the LAR, and requests a route. Decodes all the information received from the backend and builds the response that will be sent to the client. In case an error occurs the user is informed with a message.
@param mission_planning::Path::Request
contains the requested destination coordinates
@param mission_planning::Path::Response
will be the response for the client
@return a bool that indicates if the request was succesfully sent or not
*/
//ros server function that processes request data and fills response fields (service callback)
bool return_path(
        mission_planning::Path::Request &req, //contains data sent from client
        mission_planning::Path::Response &resp)
{
    double startLat = req.gpsLat;
    double startLon = req.gpsLon;
    double destLat = req.destLat;
    double destLon = req.destLon;
    //Local calculations, requires that osrm-backend is installed and running on your computer
    //string htmlquery = "http://localhost:5000/viaroute?loc="+to_string(startLat)+","+to_string(startLon)+"&loc="+to_string(destLat)+","+to_string(destLon)+"&instructions=true";

    //Remote calculations, requires that osrm-backend is installed and running on the LAR server
    string htmlquery = "http://193.137.172.18:5000/viaroute?loc="+to_string(startLat)+","+to_string(startLon)+"&loc="+to_string(destLat)+","+to_string(destLon)+"&instructions=true";

    /*
}
int main(){
*/
    //STEP 1 - GET JSON VIA CURL
    int http_code = 0;
    string data;

    CURL *curl;
    CURLcode res;

    //Initialize cURL
    curl = curl_easy_init();

    if(curl){

        //Set the function to call when there is new data
        curl_easy_setopt(curl,CURLOPT_WRITEFUNCTION, write_to_string);

        //Set the parameter to append the new data to
        curl_easy_setopt(curl, CURLOPT_WRITEDATA, &data);

        //Set the URL to download
        curl_easy_setopt(curl, CURLOPT_URL, htmlquery.c_str());
        //curl_easy_setopt(curl, CURLOPT_URL, "http://localhost:5000/viaroute?loc=40.63179,-8.65712&loc=40.64382,-8.64238&instructions=true");

        /* Follow HTTP redirection in necessary */
        curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L);

        /* Perform the request, res will get the return code (Downloads content) */
        res = curl_easy_perform(curl);
        /* Check for errors */
        if(res != CURLE_OK)
            fprintf(stderr, "curl_easy_perform() failed: %s\n",
                    curl_easy_strerror(res));

        //Get the HTTP response code
        curl_easy_getinfo(curl,CURLINFO_RESPONSE_CODE, &http_code);

        /* Always cleanup */
        curl_easy_cleanup(curl);
        curl_global_cleanup();
    }

    //Output response as a string
    // cout << "Server Response Code: "<< http_code << "\nJSON FILE:\n"<< data << endl;

    //STEP 2 - PARSE JSON USING JSONCPP LIB
    //HTTP CODE 200 == OK
    if (http_code == 200){

        std::cout << "\nServer OK, JSON file acquired " << std::endl;
        // Response looks good - done using Curl now.  Try to parse the results and print them out.
        Json::Value jsonData;
        Json::Reader jsonReader;

        if (jsonReader.parse(data, jsonData))

            std::cout << "Successfully parsed JSON data" << std::endl;
        std::cout << "\nJSON data received:" << std::endl;
        std::cout << jsonData.toStyledString() << std::endl;

        //In case the gps looses signal and requests a destination based from lat=0.000 lon=0.000
         Json::Value invalid_coord = jsonData["status"];
         if (invalid_coord == 207){printf("Error no route found between the requested points, Check Gps Signal\n");exit(0);}
        //

        //GET encodedPolyline from json
        Json::Value encodedLine = jsonData["route_geometry"];
        cout << "FIELD STATUS = " << encodedLine << "\n" << endl;

        vector<double> data = Decompress(encodedLine.asString(),6);
        vector<pair<double,double> > waypoints; //pair of lat,lon values


        for ( int i = 0; i < data.size(); i+=2) {
            waypoints.push_back(make_pair(data[i],data[i+1])); //pair of lat,lon values

            printf ("%lf , %lf \n",data[i],data[i+1]);
        }


        //GET INSTRUCTIONS
        string turnInstruction [18]={"NoTurn","GoStraight","TurnSlightRight","TurnRight","TurnSharpRight","UTurn",
                                     "TurnSharpLeft","TurnLeft","TurnSlightLeft","ReachViaLocation","HeadOn","EnterRoundAbout",
                                     "LeaveRoundAbout","StayOnRoundAbout","StartAtEndOfStreet","ReachedYourDestination",
                                     "EnterAgainstAllowedDirection","LeaveAgainstAllowedDirection"};


        Json::Value instructions = jsonData["route_instructions"];
        //cout << "INSTRUCTIONS = " << instructions << "\n" << endl;
        cout << "Size of instructions = "<< instructions.size() << endl;

        //Acess the second element of the first array
        //cout << instructions[0][1] << endl;

        //Loop through all instructions
        std::vector<int> instPos; //instruction todo at waypoint X
        std::vector<string>instTodo; // set of instruction

        for (int i = 0; i < instructions.size(); i++) {


            instPos.push_back(instructions[i][3].asInt()); //add waypoint index that has an instruction


            string dir = instructions[i][0].asString(); //convert instructions index to string
            if(dir.find("-")!=std::string::npos) //if string contains "-" (has more than one instruction)
            {
                int a = dir.find("-"); //position of "-"
                string found = dir.substr(0,a); //first instruction
                string found2 = dir.substr(a+1,dir.size()); //second instruction
                //cout << turnInstruction[atoi(found.c_str())] << " and "<< turnInstruction[atoi(found2.c_str())] << instructions[i][1] << endl;
                string fullInst = (turnInstruction[atoi(found.c_str())] + " and " + turnInstruction[atoi(found2.c_str())]);

                instTodo.push_back(fullInst); //adds complex instruction to array

            }
            else
            {
                int dirOK = atoi(dir.c_str()); //converts string to int
                //prints manouver and name of the street
                //cout << turnInstruction[dirOK] << " " << instructions[i][1] << endl;
                instTodo.push_back(turnInstruction[dirOK]); //adds simple instruction to array

            }
        }
        /*
        cout<<"TODO:"<<endl;
        for (int  i= 0;  i< instTodo.size(); i++) {
            cout << instTodo.at(i) << endl;
        }

        cout<<"Pos"<<endl;
        for (int i = 0; i < instPos.size(); ++i) {
            cout << "waypoint index: "<< instPos.at(i) << endl;
        }
*/

        cout<<"Waypoints size: "<<waypoints.size()<<endl;

        //SET of Points UNDER CONSTRUCTION
        std::vector<GpsLocation::Ptr> locations; //array of positions ( contains waypoints + driving instruction )
        //GpsLocation::Ptr loc1(new GpsLocation(0,0,"0")); //add new element
        //locations.push_back(loc1);

        //Create the mission ( Waypoints + instructions )
        for (int i = 0; i < waypoints.size(); i++) {

            vector<int>::iterator it = find(instPos.begin(),instPos.end(),i); //get iterator where position was found
            int index = it - instPos.begin(); //get real position

            if(it!=instPos.end()){
                //if contains instruction
                //printf("%d-%d %lf , %lf , %s \n",i,i+1,waypoints[i].first,waypoints[i].second,instTodo[index].c_str());
                GpsLocation::Ptr loc1(new GpsLocation(waypoints[i].first,waypoints[i].second,instTodo[index].c_str())); //add new element
                locations.push_back(loc1);
            }else{
                //printf ("%d-%d %lf , %lf \n",i,i+1,waypoints[i].first,waypoints[i].second);
                //In case there is no instruction TODO, instead of placing "" place "Go on"
                GpsLocation::Ptr loc1(new GpsLocation(waypoints[i].first,waypoints[i].second,"")); //add new element
                locations.push_back(loc1);

            }

        }

        cout << "Size of locations: " << locations.size() << endl;
        for (int i = 0; i < locations.size(); ++i) {
            //std::cout << (*locations[i]).latitude << std::endl;

            //Print locations
            GpsLocation::Ptr location = locations[i];
            cout << location << endl;

            //After filling the response fields we are able to return true

            //resp.lat[i]=(*locations[i]).latitude;
            //resp.lon[i]=(*locations[i]).longitude;
            //resp.instruction[i]=(*locations[i]).maneuver;
            resp.lat.push_back((*locations[i]).latitude);
            resp.lon.push_back((*locations[i]).longitude);
            resp.instruction.push_back((*locations[i]).maneuver);
        }

        ROS_INFO("Request complete"); //complete with data
        return true;

    } //if httpcode = 200
    else
    {
        std::cout << "Could not parse HTTP data as JSON" << std::endl;
        std::cout << "HTTP data was:\n" << data << std::endl;
        ROS_ERROR("Could not parse HTTP, failed to send request");
        return 1;
    }//if httpcode != 200


}
/*
@brief
main of this program.
creates a new node and starts a service in which this module will be the server.
*/
int main(int argc, char** argv)
{

    //ROS CODE
    ros::init(argc, argv, "mission_response_server"); //name of the node
    ros::NodeHandle nh;

    //Register the service with the master
    ros::ServiceServer server = nh.advertiseService("return_path",&return_path);//creating a server object (service_name,pointer_to_callback_function)
    ROS_INFO("Ready to send mission!");
    ros::spin(); //enters a loop, pumping callbacks. All callbacks will be called from within this thread.
    //ros::spinOnce();

    return 0;

} //end main