TSPSearch.cpp

Go to the documentation of this file.

/*
 * MDPSearch.cpp
 *
 *  Created on: 20.01.2011
 *      Author: hess
 */
#include <coverage_3d_planning/TSPSearch.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <sys/syscall.h>
#include <sys/sysctl.h>
//c++
#include <cmath>
#include <limits>
#include <vector>
#include <algorithm>
#include <ostream>
#include <fstream>

#include <limits>

#include <Eigen/Core>

//TSPSearch::TSPSearch() :
//        file_tsp_result_("/home/hess/coverage.result"), file_tsp_problem_("/home/hess/coverage.tsp"), file_tsp_param_(
//                "/home/hess/coverage.par") {
//
//}

TSPSearch::TSPSearch() :
        file_tsp_result_("/tmp/coverage.result"), file_tsp_problem_("/tmp/coverage.tsp"), file_tsp_param_(
                "/tmp/coverage.par") {

    std::time_t seconds;
    std::time(&seconds);
    std::srand((unsigned int) seconds);

    std::stringstream sstream;
    sstream << "/tmp/coverage_3d_" << std::rand() << std::rand();
    file_tsp_result_ = sstream.str();
    file_tsp_result_+=".result";
    file_tsp_problem_ = sstream.str();
    file_tsp_problem_ += ".tsp";
    file_tsp_param_ = sstream.str();
    file_tsp_param_ += ".par";
}

void TSPSearch::writeToTSPFile(const Eigen::MatrixXf& weights) {

    //write parameter file: .par
    //PROBLEM_FILE = tsp1.tsp
    //OUTPUT_TOUR_FILE = output.tsp
    //MOVE_TYPE = 5
    //PATCHING_C = 3
    //PATCHING_A = 2
    //RUNS = 1
    ofstream file;
    ROS_DEBUG_STREAM("TSPSearch::writeToTSPFile: TSP file" << file_tsp_param_);
    file.open(file_tsp_param_.c_str(), ios::out);
    file.precision(5);
    file << "PROBLEM_FILE = " << file_tsp_problem_ << std::endl;
    file << "OUTPUT_TOUR_FILE = " << file_tsp_result_ << std::endl;
    file << "MOVE_TYPE = 5" << std::endl;
    file << "PATCHING_C = 3" << std::endl;
    file << "PATCHING_A = 2" << std::endl;
    file << "PRECISION = 1" << std::endl;
    file << "RUNS = 1" << std::endl;
    file << "SEED = "<< rand() << std::endl;
    file.close();

    //write problem file: .tsp
    //NAME : tsp1
    //COMMENT : test problem
    //TYPE : ATSP
    //DIMENSION : 4
    //EDGE_WEIGHT_TYPE : EXPLICIT
    //EDGE_WEIGHT_FORMAT : FULL_MATRIX
    //EDGE_WEIGHT_SECTION :
    //0 9 8 5 1 0 8 6 9 1 0 4 1 2 3 0
    //EOF

//      ROS_INFO_STREAM("filename out2 " << tsp_file.str().c_str());
    file.open(file_tsp_problem_.c_str(), ios::out);
    file.precision(5);
    file << "NAME : WIPE_TSP" << std::endl;
    file << "TYPE : ATSP" << std::endl;
    file << "DIMENSION : " << weights.rows() << std::endl;
    file << "EDGE_WEIGHT_TYPE : EXPLICIT" << std::endl;
    file << "EDGE_WEIGHT_FORMAT : FULL_MATRIX" << std::endl;
//    file << "EDGE_DATA_FORMAT : ADJ_LIST" << std::endl;
//
//    file << "EDGE_DATA_SECTION : " << std::endl;
//    //one more node for current tool position
//    for (unsigned int i = 0; i < weights.rows(); i++) {
//        file << i+1 << " ";
//        for (unsigned int j = 0; j < weights.cols(); j++) {
//            if (weights(i,j) > 0.00001){
//                file << j+1 << " ";
//            }
//
//        }
//        if(i ==weights.rows()-1){
//            file << "-1 -1" << endl;
//        } else {
//            file << "-1" << endl;
//        }
//
//    }
//    file << "-1" << endl;

    file << "EDGE_WEIGHT_SECTION : " << std::endl;
    //one more node for current tool position
    for (unsigned int i = 0; i < weights.rows(); i++) {
        for (unsigned int j = 0; j < weights.cols(); j++) {
//            if (weights(i,j) < 0.000000001){
//                file << 100 << " ";
//            } else {
                        int tmp =static_cast<int>(weights(i, j)*100);
                file << tmp << " ";
                if (tmp==-2147483648){
                        std::cout<< "overflow "<<  weights(i, j) << " i " <<i << " j " <<j << std::endl;
                }
//            }

//                      std::cout <<  static_cast<int>(edge_weights_[i][j] * 100) <<  " "  << edge_weights_[i][j] << std::endl;
        }
    }
    file << std::endl;
    file << "EOF";
    file.close();

}
bool TSPSearch::callTSPSolver() {

    stringstream command;
//      command << lkh_bin_dir_ <<"LKH " <<  file_tsp_param_;
    command << "rosrun lkh_solver LKH " << file_tsp_param_;
    int result = system(command.str().c_str());
    ROS_INFO_STREAM("result " << result);
    return true;
}

void TSPSearch::tokenize(const string& str, vector<string>& tokens, const string& delimiters) {
    string::size_type last_pos = str.find_first_not_of(delimiters, 0);
    string::size_type pos = str.find_first_of(delimiters, last_pos);

    while (string::npos != pos || string::npos != last_pos) {
        tokens.push_back(str.substr(last_pos, pos - last_pos));

        last_pos = str.find_first_not_of(delimiters, pos);
        pos = str.find_first_of(delimiters, last_pos);
    }
}

void TSPSearch::readResultTSPFile(std::vector<unsigned int>& traj) {

    //NAME : tsp1.10.tour
    //COMMENT : Length = 10
    //COMMENT : Found by LKH [Keld Helsgaun]
    //TYPE : TOUR
    //DIMENSION : 4
    //TOUR_SECTION
    //1
    //4
    //3
    //2
    //-1
    //EOF
//TODO:read in the file

    traj.clear();
    ifstream file(file_tsp_result_.c_str(), ios::in);

//      ROS_INFO_STREAM("filename in " << filename.str().c_str());
    bool ready = false;
    std::string line;
    std::vector<std::string> tokens;
    if (file.good()) {

        while (!file.eof()) {
            std::getline(file, line);

//                      ROS_INFO_STREAM("line " << line);
            if (ready) {
                int node = boost::lexical_cast<int>(line);
                if (node == -1) {
                    ROS_INFO_STREAM("[TSPSearch] Read end of tour.");
                    break;
                }
                node = node - 1; //TODO: -2 if arm position inserted
                if (node >= 0) { //do not consider first node as we are there already
                    unsigned int tmp = static_cast<unsigned int>(node);
                    traj.push_back(tmp);
//                                      ROS_INFO_STREAM("node " << tmp);
                }
            }
//              ROS_INFO_STREAM("compare " << strcmp(line.c_str(),"TOUR_SECTION"));
            std::string tour = "TOUR_SECTION";
            if (line == tour)
                ready = true;
//              ROS_INFO_STREAM("ready " << ready);
        }
    } else {
        ROS_WARN_STREAM("[TSPSearch] Could not read TSP result.");
    }

//    traj.pop_back();// delete last element - as it goes to an artificial node
}