quality improvement for 1-grid-based TSP, nearest neighbor is fastest…

… in total traveling time

ipa_building_navigation/common/include/ipa_building_navigation/concorde_TSP.h

@@ -50,16 +50,17 @@ class ConcordeTSPSolver
 	AStarPlanner pathplanner_;
 
 	//Function to create neccessary TSPlib file to tell concorde what the problem is.
-	void writeToFile(const cv::Mat& pathlength_matrix);
+	void writeToFile(const cv::Mat& pathlength_matrix, const std::string& tsp_lib_filename, const std::string& tsp_order_filename);
 
 	//Function to read the saved TSP order.
-	std::vector<int> readFromFile();
+	std::vector<int> readFromFile(const std::string& tsp_order_filename);
 
 	void distance_matrix_thread(DistanceMatrix& distance_matrix_computation, cv::Mat& distance_matrix,
 			const cv::Mat& original_map, const std::vector<cv::Point>& points, double downsampling_factor,
 			double robot_radius, double map_resolution, AStarPlanner& path_planner);
 
 	bool abort_computation_;
+	std::string unique_file_identifier_;
 
 public:
 	//Constructor

ipa_building_navigation/common/src/concorde_TSP.cpp

@@ -2,6 +2,7 @@
 
 #include <boost/thread.hpp>
 #include <boost/chrono.hpp>
+#include <sys/time.h>
 
 //Default constructor
 ConcordeTSPSolver::ConcordeTSPSolver()
@@ -23,9 +24,10 @@ void ConcordeTSPSolver::abortComputation()
 	abort_computation_ = true;
 
 	// kill concorde if running
-	std::string pid_cmd = "pidof concorde > concorde_tsp_pid.txt";
+	const std::string pid_filename = "concorde_tsp_pid" + unique_file_identifier_ + ".txt";
+	std::string pid_cmd = "pidof concorde > " + pid_filename;
 	int pid_result = system(pid_cmd.c_str());
-	std::ifstream pid_reader("concorde_tsp_pid.txt");
+	std::ifstream pid_reader(pid_filename.c_str());
 	int value = -1;
 	std::string line;
 	if (pid_reader.is_open())
@@ -44,26 +46,25 @@ void ConcordeTSPSolver::abortComputation()
 			}
 		}
 		pid_reader.close();
-		remove("concorde_tsp_pid.txt");
+		remove(pid_filename.c_str());
 	}
 }
 
 //This function generates a file with the current TSP in TSPlib format. This is necessary because concorde needs this file
 //as input to solve the TSP. See http://comopt.ifi.uni-heidelberg.de/software/TSPLIB95/ for documentation.
-void ConcordeTSPSolver::writeToFile(const cv::Mat& pathlength_matrix)
+void ConcordeTSPSolver::writeToFile(const cv::Mat& pathlength_matrix, const std::string& tsp_lib_filename, const std::string& tsp_order_filename)
 {
-	std::string path_for_saving_file = "TSPlib_file.txt";//ros::package::getPath("libconcorde_tsp_solver") + "/common/files/TSPlib_file.txt";
+	const std::string path_for_saving_file = tsp_lib_filename;   //ros::package::getPath("libconcorde_tsp_solver") + "/common/files/TSPlib_file.txt";
 	std::ofstream saving_file(path_for_saving_file.c_str());
 	if (saving_file.is_open())
 	{
-		std::cout << "Starting to create the TSPlib file." << std::endl;
-		//specify name of the Problem, Type (TSP = symmetrical TSP) and add a comment to the file. Name and Type are neccessary, comment is for better understanding when you open the file.
-		// todo: maybe have this name and file names as a parameter
-		saving_file << "NAME: ipa-building-navigation" << std::endl
+		std::cout << "Starting to create the TSPlib file: " << path_for_saving_file << std::endl;
+		//specify name of the Problem, Type (TSP = symmetrical TSP) and add a comment to the file. Name and Type are necessary, comment is for better understanding when you open the file.
+		saving_file << "NAME: ipa-building-navigation_" << tsp_lib_filename << std::endl
 				<< "TYPE: TSP" << std::endl
 				<< "COMMENT: This is the TSPlib file for using concorde. See http://comopt.ifi.uni-heidelberg.de/software/TSPLIB95/ for documentation."
 				<< std::endl;
-		saving_file << "DIMENSION: " << pathlength_matrix.cols << std::endl; //Shows the Dimension of the problem --> the number of nodes (Neccessary).
+		saving_file << "DIMENSION: " << pathlength_matrix.cols << std::endl; //Shows the Dimension of the problem --> the number of nodes (Necessary).
 		//Write the distance-matrix into the file as a full-matrix.
 		saving_file << "EDGE_WEIGHT_TYPE: EXPLICIT" << std::endl;
 		saving_file << "EDGE_WEIGHT_FORMAT: FULL_MATRIX" << std::endl;
@@ -90,25 +91,24 @@ void ConcordeTSPSolver::writeToFile(const cv::Mat& pathlength_matrix)
 	}
 
 	// clear results file
-	std::string path_for_order_file = "TSP_order.txt";
-	std::ofstream reading_file(path_for_order_file.c_str()); //open file
+	std::ofstream reading_file(tsp_order_filename.c_str()); //open file
 	if (reading_file.is_open())
 	{
 		reading_file << "";
 		reading_file.close();
 	}
 	else
 	{
-		std::cout << "Could not clear results file '" << path_for_order_file << "'." << std::endl;
+		std::cout << "Could not clear results file '" << tsp_order_filename << "'." << std::endl;
 	}
 }
 
 //This function opens the file which saves the output from the concorde solver and reads the saved order. The names of the
 //nodes in the graph are stored as positions in the distance matrix in this case. The first integer in the file is the number
 //of nodes of this problem, so this one is not necessary.
-std::vector<int> ConcordeTSPSolver::readFromFile()
+std::vector<int> ConcordeTSPSolver::readFromFile(const std::string& tsp_order_filename)
 {
-	std::string path_for_order_file = "TSP_order.txt";//ros::package::getPath("libconcorde_tsp_solver") + "/common/files/TSP_order.txt"; //get path to file
+	std::string path_for_order_file = tsp_order_filename; //ros::package::getPath("libconcorde_tsp_solver") + "/common/files/TSP_order.txt"; //get path to file
 	std::ifstream reading_file(path_for_order_file.c_str()); //open file
 
 	std::vector<int> order_vector; //vector that stores the calculated TSP order
@@ -151,39 +151,49 @@ std::vector<int> ConcordeTSPSolver::readFromFile()
 //with a given distance matrix
 std::vector<int> ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& path_length_matrix, const int start_Node)
 {
+	// generate a unique filename
+	timeval time;
+	gettimeofday(&time, NULL);
+	std::stringstream ss;
+	ss << "_" << time.tv_sec << "_" << time.tv_usec;
+	unique_file_identifier_ = ss.str();
+	const std::string tsp_lib_filename = "TSPlib_file" + unique_file_identifier_ + ".txt";
+	const std::string tsp_order_filename = "TSP_order" + unique_file_identifier_ + ".txt";
+
 	std::vector<int> unsorted_order, sorted_order;
 	std::cout << "finding optimal order" << std::endl;
 	std::cout << "number of nodes: " << path_length_matrix.rows << " start node: " << start_Node << std::endl;
 	if (path_length_matrix.rows > 2) //check if the TSP has at least 3 nodes
 	{
 		//create the TSPlib file
-		writeToFile(path_length_matrix);
+		writeToFile(path_length_matrix, tsp_lib_filename, tsp_order_filename);
 
 		//use concorde to find optimal tour
 		std::string bin_folder;
 		while (bin_folder.length()==0)
 		{
+			const std::string temp_file = "temp_libconcorde_path" + unique_file_identifier_ + ".txt";
 			try
 			{
-				std::string cmd = "rospack libs-only-L libconcorde_tsp_solver > temp_libconcorde_path.txt";
+				std::string cmd = "rospack libs-only-L libconcorde_tsp_solver > " + temp_file;
 				int result = system(cmd.c_str());
-				std::ifstream file("temp_libconcorde_path.txt", std::ifstream::in);
+				std::ifstream file(temp_file.c_str(), std::ifstream::in);
 				if (file.is_open())
 				{
 					file >> bin_folder;
 					file.close();
 				}
 				std::cout << "bin_folder: " << bin_folder << std::endl;
-				remove("temp_libconcorde_path.txt");
+				remove(temp_file.c_str());
 				//bin_folder = ros::package::command("libs-only-L libconcorde_tsp_solver"); // this command crashes sometimes
 				//bin_folder.erase(std::remove(bin_folder.begin(), bin_folder.end(), '\n'));
 			}
 			catch (...)
 			{
-				std::cout << "ConcordeTSPSolver::solveConcordeTSP: ERROR: ros::package::command('libs-only-L libconcorde_tsp_solver') failed. Trying again." << std::endl;
+				std::cout << "ConcordeTSPSolver::solveConcordeTSP: ERROR: ['rospack libs-only-L libconcorde_tsp_solver > '" << temp_file << "] failed. Trying again." << std::endl;
 			}
 		}
-		std::string cmd = bin_folder + "/libconcorde_tsp_solver/concorde -o " + "$HOME/.ros/TSP_order.txt $HOME/.ros/TSPlib_file.txt";
+		std::string cmd = bin_folder + "/libconcorde_tsp_solver/concorde -o " + "$HOME/.ros/" + tsp_order_filename + " $HOME/.ros/" + tsp_lib_filename;
 		if (abort_computation_==true)
 			return sorted_order;
 		int result = system(cmd.c_str());
@@ -192,7 +202,7 @@ std::vector<int> ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& path_length_
 		assert(!result);
 
 		//get order from saving file
-		unsorted_order = readFromFile();
+		unsorted_order = readFromFile(tsp_order_filename);
 	}
 	else
 	{
@@ -201,6 +211,13 @@ std::vector<int> ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& path_length_
 			unsorted_order.push_back(node);
 		}
 	}
+	// cleanup files
+	remove(tsp_lib_filename.c_str());
+	remove(tsp_order_filename.c_str());
+	const std::string tsp_lib_sol_filename = "TSPlib_file" + unique_file_identifier_ + ".sol";
+	remove(tsp_lib_sol_filename.c_str());
+	const std::string tsp_lib_res_filename = "TSPlib_file" + unique_file_identifier_ + ".res";
+	remove(tsp_lib_res_filename.c_str());
 	std::cout << "finished TSP" << std::endl;
 
 	// if there is an error, just set unsorted order to 1, 2, 3, ...
@@ -213,7 +230,7 @@ std::vector<int> ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& path_length_
 			unsorted_order[i] = i;
 	}
 
-	//sort the order with the start_Node at the beginning
+	//sort the order with the start_node at the beginning
 	unsigned int start_node_position;
 
 	for (unsigned int i = 0; i < unsorted_order.size(); i++) //find position of the start node in the order
@@ -224,7 +241,7 @@ std::vector<int> ConcordeTSPSolver::solveConcordeTSP(const cv::Mat& path_length_
 		}
 	}
 
-	for (unsigned int i = start_node_position; i < unsorted_order.size(); i++) //sort the vector starting at start_Node
+	for (unsigned int i = start_node_position; i < unsorted_order.size(); i++) //sort the vector starting at start_node
 	{
 		sorted_order.push_back(unsorted_order[i]);
 	}

ipa_building_navigation/common/src/nearest_neighbor_TSP.cpp

@@ -65,8 +65,6 @@ std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& origin
 	DistanceMatrix distance_matrix_computation;
 	distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_);
 
-	// todo: and do not forget to copy fix to ipa_building_navigation
-
 	return solveNearestTSP(distance_matrix_ref, start_node);
 }
 

ipa_room_exploration/common/src/grid_point_explorator.cpp

@@ -113,7 +113,7 @@ void GridPointExplorator::getExplorationPath(const cv::Mat& room_map, std::vecto
 	cv::Mat inflated_rotated_room_map;
 	BoustrophedonGrid grid_lines;
 	GridGenerator::generateBoustrophedonGrid(rotated_room_map, inflated_rotated_room_map, half_cell_size, grid_lines, cv::Vec4i(0, 0, 0, 0),
-			cell_size, half_cell_size, cell_size);
+			cell_size, half_cell_size, cell_size-1);		// using cell_size-1 instead of cell_size for grid_spacing_horizontal helps the TSP planner to avoid unnecessary rotations
 	// convert grid points format
 	for (BoustrophedonGrid::iterator line=grid_lines.begin(); line!=grid_lines.end(); ++line)
 	{

ipa_room_exploration/ros/src/room_exploration_evaluation.cpp

@@ -1512,9 +1512,9 @@ int main(int argc, char **argv)
 //	map_names.push_back("lab_c_scan");
 //	map_names.push_back("Freiburg52_scan");
 //	map_names.push_back("Freiburg79_scan");
-	map_names.push_back("lab_b_scan");
-	map_names.push_back("lab_intel");
-	map_names.push_back("Freiburg101_scan");
+//	map_names.push_back("lab_b_scan");
+//	map_names.push_back("lab_intel");
+//	map_names.push_back("Freiburg101_scan");
 //	map_names.push_back("lab_d_scan");
 //	map_names.push_back("lab_f_scan");
 //	map_names.push_back("lab_a_scan");
@@ -1525,7 +1525,7 @@ int main(int argc, char **argv)
 //	map_names.push_back("office_d");
 //	map_names.push_back("office_e");
 //	map_names.push_back("office_f");
-//	map_names.push_back("office_g");
+	map_names.push_back("office_g");
 //	map_names.push_back("office_h");
 //	map_names.push_back("office_i");
 //	map_names.push_back("lab_ipa_furnitures");
@@ -1596,7 +1596,7 @@ int main(int argc, char **argv)
 	const float map_resolution = 0.05;		// [m/cell]
 
 	ExplorationEvaluation ev(nh, test_map_path, map_names, map_resolution, data_storage_path, robot_radius, coverage_radius, fov_points, planning_mode,
-			exploration_algorithms, robot_speed, robot_rotation_speed, true, false);
+			exploration_algorithms, robot_speed, robot_rotation_speed, true, true);
 	ros::shutdown();
 
 	//exit