diff --git a/install/prepare-jenkins-slave.sh b/install/prepare-jenkins-slave.sh index 11056ba..ebcd7dc 100755 --- a/install/prepare-jenkins-slave.sh +++ b/install/prepare-jenkins-slave.sh @@ -9,7 +9,6 @@ sudo apt-get install -y python3-wstool python3-catkin-tools # Package dependencies. echo "Installing CGAL dependencies." sudo apt-get install -y libcgal-dev -echo "Installing MONO dependencies." -sudo apt-get install -y mono-devel +sudo apt-get install -y libgmp-dev libmpfr-dev echo "Installig GLOG dependencices." sudo apt-get install -y libgoogle-glog-dev diff --git a/polygon_coverage_planners/CMakeLists.txt b/polygon_coverage_planners/CMakeLists.txt index c708d2d..33f24c2 100644 --- a/polygon_coverage_planners/CMakeLists.txt +++ b/polygon_coverage_planners/CMakeLists.txt @@ -11,10 +11,6 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") set(CMAKE_BUILD_TYPE Release) -# TODO(rikba): Make catkin package. -find_package(PkgConfig) -pkg_check_modules(MONO mono-2 REQUIRED) -include_directories(${MONO_INCLUDE_DIRS}) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} diff --git a/polygon_coverage_planners/src/graphs/sweep_plan_graph.cc b/polygon_coverage_planners/src/graphs/sweep_plan_graph.cc index aeb41e2..6244fd3 100644 --- a/polygon_coverage_planners/src/graphs/sweep_plan_graph.cc +++ b/polygon_coverage_planners/src/graphs/sweep_plan_graph.cc @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include @@ -522,20 +522,20 @@ bool SweepPlanGraph::solve(const Point_2& start, const Point_2& goal, const size_t goal_idx = temp_gtsp_graph.size() - 1; const size_t start_idx = temp_gtsp_graph.size() - 2; - // Solve using GK MA. + // Solve using GLKH. std::vector> m = temp_gtsp_graph.getAdjacencyMatrix(); std::vector> clusters; if (!temp_gtsp_graph.getClusters(&clusters)) { ROS_ERROR("Cannot get clusters."); return false; } - gk_ma::Task task(m, clusters); - gk_ma::GkMa& solver = gk_ma::GkMa::getInstance(); + glkh::Task task(m, clusters); + glkh::Glkh& solver = glkh::Glkh::getInstance(); solver.setSolver(task); ROS_INFO("Start solving GTSP"); if (!solver.solve()) { - ROS_ERROR("GkMa solution failed."); + ROS_ERROR("GLKH solution failed."); return false; } ROS_INFO("Finished solving GTSP"); diff --git a/polygon_coverage_planners/src/planners/polygon_stripmap_planner.cc b/polygon_coverage_planners/src/planners/polygon_stripmap_planner.cc index 4eb8136..6f56ec2 100644 --- a/polygon_coverage_planners/src/planners/polygon_stripmap_planner.cc +++ b/polygon_coverage_planners/src/planners/polygon_stripmap_planner.cc @@ -99,7 +99,7 @@ bool PolygonStripmapPlanner::runSolver(const Point_2& start, std::vector* solution) const { ROS_ASSERT(solution); - ROS_INFO("Start solving GTSP using GK MA."); + ROS_INFO("Start solving GTSP using GLKH."); return sweep_plan_graph_.solve(start, goal, solution); } diff --git a/polygon_coverage_solvers/CMakeLists.txt b/polygon_coverage_solvers/CMakeLists.txt index f5d69f4..d4d3954 100644 --- a/polygon_coverage_solvers/CMakeLists.txt +++ b/polygon_coverage_solvers/CMakeLists.txt @@ -13,49 +13,18 @@ catkin_package( ) include_directories(include ${catkin_INCLUDE_DIRS}) -# Add mono to invoke gk_ma. -find_package(PkgConfig) -pkg_check_modules(MONO mono-2 REQUIRED) -include_directories(${MONO_INCLUDE_DIRS}) - -# Download gk_ma -set(GKMA_INCLUDE_DIR ${CMAKE_BINARY_DIR}/gk_ma-prefix/src/gk_ma) +# Download glkh include(ExternalProject) ExternalProject_Add( - gk_ma - URL http://www.cs.nott.ac.uk/~pszdk/gtsp_ma_source_codes.zip - URL https://polybox.ethz.ch/index.php/s/H4NXeaNPWo6VBrf/download - DOWNLOAD_NAME gtsp_ma_source_codes.zip - URL_MD5 765fad8e3746fa3dd9b81be0afb34d35 + glkh + URL http://webhotel4.ruc.dk/~keld/research/GLKH/GLKH-1.0.tgz + DOWNLOAD_NAME glkh_source_codes.tgz + URL_MD5 9b0ba92053dac798f550c5c8a9524120 + BINARY_DIR ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION} PATCH_COMMAND - COMMAND patch GkMa/OurHeuristic/Algorithm.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Algorithm.patch - COMMAND patch NativeHelper/ClusterOptimisation.cpp ${CMAKE_CURRENT_SOURCE_DIR}/patches/ClusterOptimisationCpp.patch - COMMAND patch GkMa/OurHeuristic/Types/Generation.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Generation.patch - COMMAND patch GkMa/OurHeuristic/GeneticAlgorithm.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/GeneticAlgorithm.patch - COMMAND patch GkMa/Helper.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Helper.patch - COMMAND patch NativeHelper/ClusterOptimisation.h ${CMAKE_CURRENT_SOURCE_DIR}/patches/ClusterOptimisationH.patch - COMMAND patch NativeHelper/ImprovementManager.h ${CMAKE_CURRENT_SOURCE_DIR}/patches/ImprovementManagerH.patch - COMMAND patch NativeHelper/Insert.cpp ${CMAKE_CURRENT_SOURCE_DIR}/patches/InsertCpp.patch - COMMAND patch NativeHelper/Insert.h ${CMAKE_CURRENT_SOURCE_DIR}/patches/InsertH.patch - COMMAND patch NativeHelper/NativeHelper.cpp ${CMAKE_CURRENT_SOURCE_DIR}/patches/NativeHelperCpp.patch - COMMAND patch NativeHelper/Swap.cpp ${CMAKE_CURRENT_SOURCE_DIR}/patches/SwapCpp.patch - COMMAND patch NativeHelper/NativeHelper.h ${CMAKE_CURRENT_SOURCE_DIR}/patches/NativeHelperH.patch - COMMAND patch GkMa/OurHeuristic/NativeHelper.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/NativeHelper.patch - COMMAND patch GkMa/OurSolver.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/OurSolver.patch - COMMAND patch GkMa/OurHeuristic/Types/Permutation.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Permutation.patch - COMMAND patch GkMa/Program.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Program.patch - COMMAND patch GkMa/Solver.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Solver.patch - COMMAND patch GkMa/Loader/Task.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Task.patch - COMMAND patch GkMa/OurHeuristic/Types/Tour.cs ${CMAKE_CURRENT_SOURCE_DIR}/patches/Tour.patch - UPDATE_COMMAND "" - CONFIGURE_COMMAND - COMMAND cp ${PROJECT_SOURCE_DIR}/patches/MakefileCpp ./MakefileCpp - COMMAND cp ${PROJECT_SOURCE_DIR}/patches/MakefileCs ./MakefileCs - BUILD_COMMAND - COMMAND $(MAKE) -f MakefileCs BUILD_PATH="${CMAKE_LIBRARY_OUTPUT_DIRECTORY}" - COMMAND $(MAKE) -f MakefileCpp BUILD_PATH="${CMAKE_LIBRARY_OUTPUT_DIRECTORY}" - INSTALL_COMMAND - COMMAND ${CMAKE_COMMAND} -E create_symlink /usr/lib/libmono-native.so ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}/System.Native + COMMAND patch --forward ./SRC/SolveTSP.c < ${PROJECT_SOURCE_DIR}/patches/SolveTSP.patch + COMMAND cp ${PROJECT_SOURCE_DIR}/patches/CMakeLists.txt ./ + INSTALL_COMMAND "" ) # Download GTSP test instances. @@ -76,20 +45,23 @@ ExternalProject_Add( # LIBRARIES # ############# add_library(${PROJECT_NAME} - src/gk_ma.cc + src/glkh.cc src/combinatorics.cc src/boolean_lattice.cc ) -target_link_libraries(${PROJECT_NAME} ${MONO_LIBRARIES} ${catkin_LIBRARIES}) + +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) ######### # TESTS # ######### -catkin_add_gtest(test_combinatorics test/combinatorics-test.cpp) +catkin_add_gtest(test_combinatorics + test/combinatorics-test.cpp +) target_link_libraries(test_combinatorics ${PROJECT_NAME} ${catkin_LIBRARIES}) -catkin_add_gtest(test_gk_ma test/gk_ma-test.cpp) -target_link_libraries(test_gk_ma ${PROJECT_NAME} ${catkin_LIBRARIES}) +catkin_add_gtest(test_glkh test/glkh-test.cpp) +target_link_libraries(test_glkh ${PROJECT_NAME} ${catkin_LIBRARIES}) ########## diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/gk_ma.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/glkh.h similarity index 67% rename from polygon_coverage_solvers/include/polygon_coverage_solvers/gk_ma.h rename to polygon_coverage_solvers/include/polygon_coverage_solvers/glkh.h index 8883861..3a24c65 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/gk_ma.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/glkh.h @@ -17,17 +17,15 @@ * this program. If not, see . */ -#ifndef POLYGON_COVERAGE_SOLVERS_GK_MA_H_ -#define POLYGON_COVERAGE_SOLVERS_GK_MA_H_ +#ifndef POLYGON_COVERAGE_SOLVERS_GLKH_H_ +#define POLYGON_COVERAGE_SOLVERS_GLKH_H_ #include #include -#include - -// Interfaces with the GK MA GTSP solver. +// Interfaces with the GLKH GTSP solver. namespace polygon_coverage_planning { -namespace gk_ma { +namespace glkh { struct Task { Task(const std::vector>& m, const std::vector>& clusters) @@ -38,37 +36,29 @@ struct Task { std::vector> clusters; }; -// References GkMa.exe. Singleton, because it may only be referenced once during +// References GLKH. Singleton, because it may only be referenced once during // runtime. // https://stackoverflow.com/questions/1008019/c-singleton-design-pattern -class GkMa { +class Glkh { public: - inline static GkMa& getInstance() { - static GkMa instance; + inline static Glkh& getInstance() { + static Glkh instance; return instance; } - GkMa(GkMa const&) = delete; - void operator=(GkMa const&) = delete; + Glkh(Glkh const&) = delete; + void operator=(Glkh const&) = delete; - void setSolver(const std::string& file, bool binary); void setSolver(const Task& task); bool solve(); inline std::vector getSolution() const { return solution_; } private: - GkMa(); - ~GkMa(); - - MonoArray* vectorOfVectorToMonoArray( - const std::vector>& in) const; - - MonoDomain* domain_; - MonoObject* solver_; - MonoClass* solver_class_; + Glkh(); + ~Glkh() = default; std::vector solution_; }; -} // namespace gk_ma +} // namespace glkh } // namespace polygon_coverage_planning -#endif // POLYGON_COVERAGE_SOLVERS_GK_MA_H_ +#endif // POLYGON_COVERAGE_SOLVERS_GLKH_H_ diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h index 2ad91be..5c19b94 100644 --- a/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/impl/graph_base_impl.h @@ -388,7 +388,7 @@ GraphBase::getAdjacencyMatrix() const { if (edgeExists(edge) && getEdgeCost(edge, &cost)) { m[i][j] = static_cast(cost * scale); } else { - m[i][j] = std::numeric_limits::max(); + m[i][j] = std::numeric_limits::max(); // TODO: overflow! } } } diff --git a/polygon_coverage_solvers/include/polygon_coverage_solvers/utils.h b/polygon_coverage_solvers/include/polygon_coverage_solvers/utils.h new file mode 100644 index 0000000..4572458 --- /dev/null +++ b/polygon_coverage_solvers/include/polygon_coverage_solvers/utils.h @@ -0,0 +1,34 @@ + + +#ifndef POLYGON_COVERAGE_SOLVERS_UTILS_H_ +#define POLYGON_COVERAGE_SOLVERS_UTILS_H_ + +#include +#include +#include + +namespace polygon_coverage_planning { + +// trim from start (in place) +static inline void ltrim(std::string &s) { + s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) { + return !std::isspace(ch); + })); +} + +// trim from end (in place) +static inline void rtrim(std::string &s) { + s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) { + return !std::isspace(ch); + }).base(), s.end()); +} + +// trim from both ends (in place) +static inline void trim(std::string &s) { + ltrim(s); + rtrim(s); +} + +} // namespace polygon_coverage_planning + +#endif // POLYGON_COVERAGE_SOLVERS_UTILS_H_ diff --git a/polygon_coverage_solvers/patches/Algorithm.patch b/polygon_coverage_solvers/patches/Algorithm.patch deleted file mode 100644 index cddf0ac..0000000 --- a/polygon_coverage_solvers/patches/Algorithm.patch +++ /dev/null @@ -1,22 +0,0 @@ -30a31 -> -32,34c33,35 -< { -< Tour tour = new Tour(generatingAlgorithm); -< tour.Improve(); ---- -> { -> Tour tour = new Tour(generatingAlgorithm); -> tour.Improve(); -36,38c37,39 -< cur.Add(tour); -< } -< cur.Sort(); ---- -> cur.Add(tour); -> } -> cur.Sort(); -75c76 -< { ---- -> { diff --git a/polygon_coverage_solvers/patches/CMakeLists.txt b/polygon_coverage_solvers/patches/CMakeLists.txt new file mode 100644 index 0000000..e935aae --- /dev/null +++ b/polygon_coverage_solvers/patches/CMakeLists.txt @@ -0,0 +1,163 @@ +cmake_minimum_required(VERSION 3.0.2) + +project(glkh) + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel +endif() +add_compile_options(-Wall) +#set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -Wall -g") + +set(COMMON_INCLUDES + ${PROJECT_SOURCE_DIR}/SRC/INCLUDE +) + +set(COMMON_SOURCES + SRC/SolveGTSP.c + SRC/ReadParameters.c + SRC/PrintParameters.c + SRC/Distance.c + SRC/printff.c + SRC/GetTime.c + SRC/Makefile + SRC/WriteTour.c + SRC/ReadProblem.c + SRC/PostOptimize.c + SRC/SolveTSP.c + SRC/fscanint.c + SRC/Random.c + SRC/ReadLine.c + SRC/eprintf.c + SRC/Statistics.c + SRC/IsCandidate.c + SRC/ReadPenalties.c + SRC/Distance_SPECIAL.c +) + +add_executable(LKH + LKH-2.0.7/SRC/Heap.c + LKH-2.0.7/SRC/GenerateCandidates.c + LKH-2.0.7/SRC/SolveSubproblemBorderProblems.c + LKH-2.0.7/SRC/GreedyTour.c + LKH-2.0.7/SRC/SolveSFCSubproblems.c + LKH-2.0.7/SRC/NormalizeNodeList.c + LKH-2.0.7/SRC/ReadCandidates.c + LKH-2.0.7/SRC/ChooseInitialTour.c + LKH-2.0.7/SRC/IsCommonEdge.c + LKH-2.0.7/SRC/Make2OptMove.c + LKH-2.0.7/SRC/SolveRoheSubproblems.c + LKH-2.0.7/SRC/Flip_SSL.c + LKH-2.0.7/SRC/FindTour.c + LKH-2.0.7/SRC/SolveCompressedSubproblem.c + LKH-2.0.7/SRC/Ascent.c + LKH-2.0.7/SRC/Between_SL.c + LKH-2.0.7/SRC/RecordBestTour.c + LKH-2.0.7/SRC/GeoConversion.c + LKH-2.0.7/SRC/SolveSubproblem.c + LKH-2.0.7/SRC/MakeKOptMove.c + LKH-2.0.7/SRC/FreeStructures.c + LKH-2.0.7/SRC/Activate.c + LKH-2.0.7/SRC/ReadParameters.c + LKH-2.0.7/SRC/Best3OptMove.c + LKH-2.0.7/SRC/ResetCandidateSet.c + LKH-2.0.7/SRC/PatchCycles.c + LKH-2.0.7/SRC/SolveTourSegmentSubproblems.c + LKH-2.0.7/SRC/MergeWithTour.c + LKH-2.0.7/SRC/Minimum1TreeCost.c + LKH-2.0.7/SRC/Gain23.c + LKH-2.0.7/SRC/AllocateStructures.c + LKH-2.0.7/SRC/PrintParameters.c + LKH-2.0.7/SRC/Distance.c + LKH-2.0.7/SRC/Make3OptMove.c + LKH-2.0.7/SRC/IsPossibleCandidate.c + LKH-2.0.7/SRC/CreateDelaunayCandidateSet.c + LKH-2.0.7/SRC/SegmentSize.c + LKH-2.0.7/SRC/AddTourCandidates.c + LKH-2.0.7/SRC/CreateQuadrantCandidateSet.c + LKH-2.0.7/SRC/BuildKDTree.c + LKH-2.0.7/SRC/Flip_SL.c + LKH-2.0.7/SRC/MergeTourWithBestTour.c + LKH-2.0.7/SRC/Delaunay.c + LKH-2.0.7/SRC/Genetic.c + LKH-2.0.7/SRC/CandidateReport.c + LKH-2.0.7/SRC/Sequence.c + LKH-2.0.7/SRC/RecordBetterTour.c + LKH-2.0.7/SRC/printff.c + LKH-2.0.7/SRC/LKHmain.c + LKH-2.0.7/SRC/AddExtraCandidates.c + LKH-2.0.7/SRC/GetTime.c + LKH-2.0.7/SRC/Between_SSL.c + LKH-2.0.7/SRC/Hashing.c + LKH-2.0.7/SRC/Best4OptMove.c + LKH-2.0.7/SRC/WriteTour.c + LKH-2.0.7/SRC/RestoreTour.c + LKH-2.0.7/SRC/Forbidden.c + LKH-2.0.7/SRC/SolveKCenterSubproblems.c + LKH-2.0.7/SRC/C.c + LKH-2.0.7/SRC/NormalizeSegmentList.c + LKH-2.0.7/SRC/RemoveFirstActive.c + LKH-2.0.7/SRC/MinimumSpanningTree.c + LKH-2.0.7/SRC/BridgeGain.c + LKH-2.0.7/SRC/Flip.c + LKH-2.0.7/SRC/Best5OptMove.c + LKH-2.0.7/SRC/Best2OptMove.c + LKH-2.0.7/SRC/Excludable.c + LKH-2.0.7/SRC/ReadProblem.c + LKH-2.0.7/SRC/Connect.c + LKH-2.0.7/SRC/StoreTour.c + LKH-2.0.7/SRC/Between.c + LKH-2.0.7/SRC/SolveDelaunaySubproblems.c + LKH-2.0.7/SRC/AdjustCandidateSet.c + LKH-2.0.7/SRC/LinKernighan.c + LKH-2.0.7/SRC/WriteCandidates.c + LKH-2.0.7/SRC/TrimCandidateSet.c + LKH-2.0.7/SRC/Make4OptMove.c + LKH-2.0.7/SRC/SFCTour.c + LKH-2.0.7/SRC/SymmetrizeCandidateSet.c + LKH-2.0.7/SRC/WritePenalties.c + LKH-2.0.7/SRC/fscanint.c + LKH-2.0.7/SRC/SolveKMeansSubproblems.c + LKH-2.0.7/SRC/qsort.c + LKH-2.0.7/SRC/Random.c + LKH-2.0.7/SRC/FixedOrCommonCandidates.c + LKH-2.0.7/SRC/OrderCandidateSet.c + LKH-2.0.7/SRC/IsBackboneCandidate.c + LKH-2.0.7/SRC/Make5OptMove.c + LKH-2.0.7/SRC/SolveKarpSubproblems.c + LKH-2.0.7/SRC/ERXT.c + LKH-2.0.7/SRC/ReadLine.c + LKH-2.0.7/SRC/AddCandidate.c + LKH-2.0.7/SRC/eprintf.c + LKH-2.0.7/SRC/Statistics.c + LKH-2.0.7/SRC/BestKOptMove.c + LKH-2.0.7/SRC/Exclude.c + LKH-2.0.7/SRC/IsCandidate.c + LKH-2.0.7/SRC/KSwapKick.c + LKH-2.0.7/SRC/ReadPenalties.c + LKH-2.0.7/SRC/CreateCandidateSet.c + LKH-2.0.7/SRC/Distance_SPECIAL.c +) +target_include_directories(LKH PRIVATE + ${PROJECT_SOURCE_DIR}/LKH-2.0.7/SRC/INCLUDE +) +target_link_libraries(LKH m) + +set(GLKH_SOURCES SRC/GLKHmain.c) +add_executable(GLKH ${GLKH_SOURCES} ${COMMON_SOURCES}) +target_include_directories(GLKH PRIVATE ${COMMON_INCLUDES}) +target_link_libraries(GLKH m) + +set(GLKH_EXP_SOURCES SRC/GLKH_EXPmain.c) +add_executable(GLKH_EXP ${GLKH_EXP_SOURCES} ${COMMON_SOURCES}) +target_include_directories(GLKH_EXP PRIVATE ${COMMON_INCLUDES}) +target_link_libraries(GLKH_EXP m) + +set(GLKH_CHECK_SOURCES SRC/GLKH_CHECKmain.c) +add_executable(GLKH_CHECK ${GLKH_CHECK_SOURCES} ${COMMON_SOURCES}) +target_include_directories(GLKH_CHECK PRIVATE ${COMMON_INCLUDES}) +target_link_libraries(GLKH_CHECK m) + +install(TARGETS LKH RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +install(TARGETS GLKH RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +install(TARGETS GLKH_EXP RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) +install(TARGETS GLKH_CHECK RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/polygon_coverage_solvers/patches/ClusterOptimisationCpp.patch b/polygon_coverage_solvers/patches/ClusterOptimisationCpp.patch deleted file mode 100644 index 98e9114..0000000 --- a/polygon_coverage_solvers/patches/ClusterOptimisationCpp.patch +++ /dev/null @@ -1,19 +0,0 @@ -2a3 -> #include -5,7c6,8 -< static int *solution; -< static int clusterSequence[MAX_CLUSTER_COUNT]; -< static int bestSolution[MAX_CLUSTER_COUNT]; ---- -> int *solution; -> int clusterSequence[MAX_CLUSTER_COUNT]; -> int bestSolution[MAX_CLUSTER_COUNT]; -84c85 -< } ---- -> } -138c139 -< } -\ No newline at end of file ---- -> } diff --git a/polygon_coverage_solvers/patches/ClusterOptimisationH.patch b/polygon_coverage_solvers/patches/ClusterOptimisationH.patch deleted file mode 100644 index e8c7638..0000000 --- a/polygon_coverage_solvers/patches/ClusterOptimisationH.patch +++ /dev/null @@ -1,7 +0,0 @@ -3,4c3,4 -< __declspec(dllexport) int ClusterOptimisation(int *solution); -< } -\ No newline at end of file ---- -> __attribute__ ((visibility ("default"))) int ClusterOptimisation(int *solution); -> } diff --git a/polygon_coverage_solvers/patches/Generation.patch b/polygon_coverage_solvers/patches/Generation.patch deleted file mode 100644 index 4d2ec33..0000000 --- a/polygon_coverage_solvers/patches/Generation.patch +++ /dev/null @@ -1,14 +0,0 @@ -26c26 -< counter.Start(); ---- -> // counter.Start(); -31c31 -< counter.Stop(); ---- -> // counter.Stop(); -59,60c59,60 -< tours.Add(tour); -< keysTable[tour] = true; ---- -> tours.Add(tour); -> keysTable[tour] = true; diff --git a/polygon_coverage_solvers/patches/GeneticAlgorithm.patch b/polygon_coverage_solvers/patches/GeneticAlgorithm.patch deleted file mode 100644 index c90af4e..0000000 --- a/polygon_coverage_solvers/patches/GeneticAlgorithm.patch +++ /dev/null @@ -1,8 +0,0 @@ -41,43c41,43 -< Process.GetCurrentProcess().PriorityClass = ProcessPriorityClass.High; -< Thread.CurrentThread.Priority = ThreadPriority.Highest; -< Process.GetCurrentProcess().ProcessorAffinity = (IntPtr)1; ---- -> // Process.GetCurrentProcess().PriorityClass = ProcessPriorityClass.High; -> // Thread.CurrentThread.Priority = ThreadPriority.Highest; -> // Process.GetCurrentProcess().ProcessorAffinity = (IntPtr)1; diff --git a/polygon_coverage_solvers/patches/Helper.patch b/polygon_coverage_solvers/patches/Helper.patch deleted file mode 100644 index 3bebc35..0000000 --- a/polygon_coverage_solvers/patches/Helper.patch +++ /dev/null @@ -1,4 +0,0 @@ -98c98 -< return ProgramDirectory.FullName + '\\' + fileName; ---- -> return ProgramDirectory.FullName + '/' + fileName; diff --git a/polygon_coverage_solvers/patches/ImprovementManagerH.patch b/polygon_coverage_solvers/patches/ImprovementManagerH.patch deleted file mode 100644 index b405d5a..0000000 --- a/polygon_coverage_solvers/patches/ImprovementManagerH.patch +++ /dev/null @@ -1,10 +0,0 @@ -1c1 -< extern "C" ---- -> extern "C" -3c3 -< __declspec(dllexport) int Improve(int *solution, int oldLen); ---- -> __attribute__ ((visibility ("default"))) int Improve(int *solution, int oldLen); -5d4 -< diff --git a/polygon_coverage_solvers/patches/InsertCpp.patch b/polygon_coverage_solvers/patches/InsertCpp.patch deleted file mode 100644 index 04b729c..0000000 --- a/polygon_coverage_solvers/patches/InsertCpp.patch +++ /dev/null @@ -1,44 +0,0 @@ -4a5 -> #include -16c17 -< return weights[oldPrevV][oldNextV] ---- -> return weights[oldPrevV][oldNextV] -54,55c55,56 -< -< ---- -> -> -57,58c58,59 -< if (Weight(oldPrev, oldNext) -< + MinClusterDistanceForVertices(newPrev, oldV, newNext) ---- -> if (Weight(oldPrev, oldNext) -> + MinClusterDistanceForVertices(newPrev, oldV, newNext) -64,65c65,66 -< -< ---- -> -> -108c109 -< ---- -> -138c139 -< ---- -> -165c166 -< ---- -> -180c181 -< ---- -> -209c210 -< } ---- -> } diff --git a/polygon_coverage_solvers/patches/InsertH.patch b/polygon_coverage_solvers/patches/InsertH.patch deleted file mode 100644 index 5e72806..0000000 --- a/polygon_coverage_solvers/patches/InsertH.patch +++ /dev/null @@ -1,4 +0,0 @@ -3c3 -< __declspec(dllexport) int InsertsWithCO(int *solution); ---- -> __attribute__ ((visibility ("default"))) int InsertsWithCO(int *solution); diff --git a/polygon_coverage_solvers/patches/MakefileCpp b/polygon_coverage_solvers/patches/MakefileCpp deleted file mode 100644 index c158abf..0000000 --- a/polygon_coverage_solvers/patches/MakefileCpp +++ /dev/null @@ -1,15 +0,0 @@ -BUILD_PATH = "" -OUT = $(BUILD_PATH)/libnative_helper.so -SRC = "../gk_ma/NativeHelper" - -.PHONY: - -build: $(OUT) - -$(OUT): $(OBJ) - @g++ -c -fPIC $(SRC)/ClusterOptimisation.cpp $(SRC)/ImprovementManager.cpp $(SRC)/Insert.cpp $(SRC)/KOpt.cpp $(SRC)/NativeHelper.cpp $(SRC)/Swap.cpp - @g++ ClusterOptimisation.o ImprovementManager.o Insert.o KOpt.o NativeHelper.o Swap.o -shared -o $(OUT) - -clean: - @rm -f *.o - @rm -f $(OUT) diff --git a/polygon_coverage_solvers/patches/MakefileCs b/polygon_coverage_solvers/patches/MakefileCs deleted file mode 100644 index 74bc33d..0000000 --- a/polygon_coverage_solvers/patches/MakefileCs +++ /dev/null @@ -1,16 +0,0 @@ -OUT = GkMa.exe -BUILD_PATH = "" -SRC = "../gk_ma/GkMa" - -.PHONY: - -build: $(OUT) - -$(OUT): $(OBJ) - @mcs -unsafe -out:$(BUILD_PATH)/$(OUT) -pkg:dotnet -platform:anycpu -recurse:$(SRC)/*.cs - -clean: - @rm -f $(OUT) - -run: $(OUT) - @mono $(OUT) diff --git a/polygon_coverage_solvers/patches/NativeHelper.patch b/polygon_coverage_solvers/patches/NativeHelper.patch deleted file mode 100644 index 73ea05d..0000000 --- a/polygon_coverage_solvers/patches/NativeHelper.patch +++ /dev/null @@ -1,13 +0,0 @@ -11c11 -< [DllImport("NativeHelper")] ---- -> [DllImport("native_helper")] -59c59 -< [DllImport("NativeHelper.dll")] ---- -> [DllImport("native_helper")] -68c68 -< } -\ No newline at end of file ---- -> } diff --git a/polygon_coverage_solvers/patches/NativeHelperCpp.patch b/polygon_coverage_solvers/patches/NativeHelperCpp.patch deleted file mode 100644 index dae328e..0000000 --- a/polygon_coverage_solvers/patches/NativeHelperCpp.patch +++ /dev/null @@ -1,96 +0,0 @@ -3a4 -> #include -78c79 -< ---- -> -90c91 -< inline int RandomPosition() ---- -> int RandomPosition() -95c96 -< inline int PrevPos(int pos) ---- -> int PrevPos(int pos) -103c104 -< inline int NextPos(int pos) ---- -> int NextPos(int pos) -111c112 -< inline int RestoreLargePos(int largePos) ---- -> int RestoreLargePos(int largePos) -118c119 -< inline int RestoreSmallPos(int smallPos) ---- -> int RestoreSmallPos(int smallPos) -139c140 -< inline int Weight(int v1, int v2) ---- -> int Weight(int v1, int v2) -144c145 -< inline int Weight(int v1, int v2, int v3) ---- -> int Weight(int v1, int v2, int v3) -149c150 -< inline int Weight(int v1, int v2, int v3, int v4) ---- -> int Weight(int v1, int v2, int v3, int v4) -154c155 -< inline int Weight(int v1, int v2, int v3, int v4, int v5) ---- -> int Weight(int v1, int v2, int v3, int v4, int v5) -159c160 -< inline int Weight(int v1, int v2, int v3, int v4, int v5, int v6) ---- -> int Weight(int v1, int v2, int v3, int v4, int v5, int v6) -165c166 -< inline int MinClusterDistance(int c1, int c2) ---- -> int MinClusterDistance(int c1, int c2) -170c171 -< inline int MinClusterDistance(int c1, int c2, int c3) ---- -> int MinClusterDistance(int c1, int c2, int c3) -176c177 -< inline int MinClusterDistance(int c1, int c2, int c3, int c4) ---- -> int MinClusterDistance(int c1, int c2, int c3, int c4) -183c184 -< inline int MinClusterDistance(int c1, int c2, int c3, int c4, int c5) ---- -> int MinClusterDistance(int c1, int c2, int c3, int c4, int c5) -191c192 -< inline int MinClusterDistance(int c1, int c2, int c3, int c4, int c5, int c6) ---- -> int MinClusterDistance(int c1, int c2, int c3, int c4, int c5, int c6) -200c201 -< inline int MinClusterDistanceForVertices(int v1, int v2) ---- -> int MinClusterDistanceForVertices(int v1, int v2) -205c206 -< inline int MinClusterDistanceForVertices(int v1, int v2, int v3) ---- -> int MinClusterDistanceForVertices(int v1, int v2, int v3) -208c209 -< clusterByVertex[v1], ---- -> clusterByVertex[v1], -213c214 -< inline int MinClusterDistanceForVertices(int v1, int v2, int v3, int v4) ---- -> int MinClusterDistanceForVertices(int v1, int v2, int v3, int v4) -216c217 -< clusterByVertex[v1], ---- -> clusterByVertex[v1], -232,233c233,234 -< FILE *f; -< if (fopen_s(&f, fileName, "a")) ---- -> FILE *f = fopen(fileName, "a"); -> if (f == NULL) -240c241 -< ---- -> diff --git a/polygon_coverage_solvers/patches/NativeHelperH.patch b/polygon_coverage_solvers/patches/NativeHelperH.patch deleted file mode 100644 index b32ac24..0000000 --- a/polygon_coverage_solvers/patches/NativeHelperH.patch +++ /dev/null @@ -1,10 +0,0 @@ -9c9 -< extern "C" ---- -> extern "C" -11,12c11,12 -< __declspec(dllexport) bool InitProblem(int vertexCount, int clusterCount, int *weights, int *clusterSizes, int **clusters, bool isSymmetric); -< __declspec(dllexport) bool DumbCounters(char *fileName); ---- -> __attribute__ ((visibility ("default"))) bool InitProblem(int vertexCount, int clusterCount, int *weights, int *clusterSizes, int **clusters, bool isSymmetric); -> __attribute__ ((visibility ("default"))) bool DumbCounters(char *fileName); diff --git a/polygon_coverage_solvers/patches/OurSolver.patch b/polygon_coverage_solvers/patches/OurSolver.patch deleted file mode 100644 index 75d12b7..0000000 --- a/polygon_coverage_solvers/patches/OurSolver.patch +++ /dev/null @@ -1,20 +0,0 @@ -19a20,21 -> public OurSolver(int[][] m, int[][] clusters, bool isSymmetric) : base(m, clusters, isSymmetric) {} -> -31,32c33,45 -< generationCount = solver.GenerationCount; -< } ---- -> generationCount = solver.GenerationCount; -> Solution = solver.LastGeneration[0].GetValues(); -> } -> -> public int SolutionLength -> { -> get { return solution.Length; } -> } -> -> public int SolutionAtIndex(int index) -> { -> return solution[index]; -> } diff --git a/polygon_coverage_solvers/patches/Permutation.patch b/polygon_coverage_solvers/patches/Permutation.patch deleted file mode 100644 index ecefdff..0000000 --- a/polygon_coverage_solvers/patches/Permutation.patch +++ /dev/null @@ -1,5 +0,0 @@ -121c121 -< } -\ No newline at end of file ---- -> } diff --git a/polygon_coverage_solvers/patches/Program.patch b/polygon_coverage_solvers/patches/Program.patch deleted file mode 100644 index a6cd39c..0000000 --- a/polygon_coverage_solvers/patches/Program.patch +++ /dev/null @@ -1,62 +0,0 @@ -23,58d22 -< "41gr202", -< "45ts225", -< "45tsp225", -< "46pr226", -< "46gr229", -< "53gil262", -< "53pr264", -< "56a280", -< "60pr299", -< "64lin318", -< "65rbg323", -< "72rbg358", -< "80rd400", -< "81rbg403", -< "84fl417", -< "87gr431", -< "88pr439", -< "89pcb442", -< "89rbg443", -< "99d493", -< "107ali535", -< "107att532", -< "107si535", -< "113pa561", -< "115u574", -< "115rat575", -< "131p654", -< "132d657", -< "134gr666", -< "145u724", -< "157rat783", -< "200dsj1000", -< "201pr1002", -< "207si1032", -< "212u1060", -< "217vm1084", -67c31 -< FileInfo fileInfo = new FileInfo(Helper.GetFullFileName("GTSP\\" + problemName + (binary ? ".gtsp" : ".txt"))); ---- -> FileInfo fileInfo = new FileInfo(Helper.GetFullFileName("GTSP/" + problemName + (binary ? ".gtsp" : ".txt"))); -84,85c48,49 -< writer.WriteLine( -< "{0}\t{1}\t{2}\t{3}\t{4}", ---- -> writer.Write( -> "{0}\t{1}\t{2}\t{3}\t{4} Solution:", -90c54,60 -< solver.GenerationCount); ---- -> solver.GenerationCount); -> -> foreach (int vertex in solver.Solution) -> { -> writer.Write(" " + vertex); -> } -> writer.WriteLine(""); -112c82 -< } -\ No newline at end of file ---- -> } diff --git a/polygon_coverage_solvers/patches/SolveTSP.patch b/polygon_coverage_solvers/patches/SolveTSP.patch new file mode 100644 index 0000000..44616e6 --- /dev/null +++ b/polygon_coverage_solvers/patches/SolveTSP.patch @@ -0,0 +1,11 @@ +--- ./SRC/SolveTSP.c 2014-05-07 17:05:32.000000000 +0200 ++++ ./SRC/SolveTSP_new.c 2021-08-10 15:12:54.771882638 +0200 +@@ -25,7 +25,7 @@ + char Command[256], Key[256], Buffer[256], *Line, *Keyword; + char Delimiters[] = " :=\n\t\r\f\v\xef\xbb\xbf"; + +- sprintf(Command, "./LKH %s", ParFileName); ++ sprintf(Command, "rosrun polygon_coverage_solvers LKH %s", ParFileName); + assert(p = popen(Command, "r")); + Cost = PLUS_INFINITY; + while (fgets(Buffer, sizeof(Buffer), p)) { diff --git a/polygon_coverage_solvers/patches/Solver.patch b/polygon_coverage_solvers/patches/Solver.patch deleted file mode 100644 index 2477c61..0000000 --- a/polygon_coverage_solvers/patches/Solver.patch +++ /dev/null @@ -1,21 +0,0 @@ -9c9,10 -< private int milliseconds; ---- -> private int milliseconds; -> protected int[] solution; -26a28,33 -> public int[] Solution -> { -> get { return solution; } -> protected set { solution = value; } -> } -> -30c37,42 -< } ---- -> } -> -> protected Solver(int[][] m, int[][] clusters, bool isSymmetric) -> { -> this.task = new Task(m, clusters, isSymmetric); -> } diff --git a/polygon_coverage_solvers/patches/SwapCpp.patch b/polygon_coverage_solvers/patches/SwapCpp.patch deleted file mode 100644 index e5ad7cd..0000000 --- a/polygon_coverage_solvers/patches/SwapCpp.patch +++ /dev/null @@ -1,31 +0,0 @@ -2a3,4 -> #include -> -23c25 -< int delta ---- -> int delta -101,102c103,104 -< solution + v1Pos, -< solution + v2Pos, ---- -> solution + v1Pos, -> solution + v2Pos, -339c341 -< int permutations[13][4] = ---- -> int permutations[13][4] = -389,394c391,396 -< prev, -< c[permutations[i][0]], -< c[permutations[i][1]], -< c[permutations[i][2]], -< c[permutations[i][3]], -< next, ---- -> prev, -> c[permutations[i][0]], -> c[permutations[i][1]], -> c[permutations[i][2]], -> c[permutations[i][3]], -> next, diff --git a/polygon_coverage_solvers/patches/Task.patch b/polygon_coverage_solvers/patches/Task.patch deleted file mode 100644 index f02d581..0000000 --- a/polygon_coverage_solvers/patches/Task.patch +++ /dev/null @@ -1,12 +0,0 @@ -160c160,168 -< } ---- -> } -> -> public Task(int[][] m, int[][] clusters, bool isSymmetric) -> { -> this.m = m; -> this.clusters = clusters; -> this.isSymmetric = isSymmetric; -> UpdateVerticesInfo(); -> } diff --git a/polygon_coverage_solvers/patches/Tour.patch b/polygon_coverage_solvers/patches/Tour.patch deleted file mode 100644 index 00bb550..0000000 --- a/polygon_coverage_solvers/patches/Tour.patch +++ /dev/null @@ -1,33 +0,0 @@ -100c100 -< ///A 32-bit signed integer that indicates the relative order of the objects being compared. The return value has the following meanings: Value Meaning Less than zero This object is less than the other parameter.Zero This object is equal to other. Greater than zero This object is greater than other. ---- -> ///A 32-bit signed integer that indicates the relative order of the objects being compared. The return value has the following meanings: Value Meaning Less than zero This object is less than the other parameter.Zero This object is equal to other. Greater than zero This object is greater than other. -149c149 -< { ---- -> { -234c234 -< ///A 32-bit signed integer that indicates the relative order of the objects being compared. The return value has the following meanings: Value Meaning Less than zero This object is less than the other parameter.Zero This object is equal to other. Greater than zero This object is greater than other. ---- -> ///A 32-bit signed integer that indicates the relative order of the objects being compared. The return value has the following meanings: Value Meaning Less than zero This object is less than the other parameter.Zero This object is equal to other. Greater than zero This object is greater than other. -436c436 -< ---- -> -453c453 -< int vertexIndex; ---- -> int vertexIndex; -461c461 -< /// Removes vertex from position oldPosition and ---- -> /// Removes vertex from position oldPosition and -554c554 -< [DllImport("NativeHelper.dll")] ---- -> [DllImport("native_helper")] -576c576 -< } -\ No newline at end of file ---- -> } diff --git a/polygon_coverage_solvers/scripts/run_glkh.bash b/polygon_coverage_solvers/scripts/run_glkh.bash new file mode 100755 index 0000000..a04885e --- /dev/null +++ b/polygon_coverage_solvers/scripts/run_glkh.bash @@ -0,0 +1,20 @@ +# Usage: ./runGLKH problem_file tour_file +#!/bin/bash + +par=$(basename $1).pid$$.par + +echo "PROBLEM_FILE = $1" > $par +echo "ASCENT_CANDIDATES = 500" >> $par +echo "INITIAL_PERIOD = 1000" >> $par +echo "MAX_CANDIDATES = 30" >> $par +echo "MAX_TRIALS = 1000" >> $par +echo "OUTPUT_TOUR_FILE = $2" >> $par +echo "POPULATION_SIZE = 1" >> $par +echo "PRECISION = 10" >> $par +echo "RUNS = 1" >> $par +echo "SEED = 1" >> $par +echo "TRACE_LEVEL = 1" >> $par + +mkdir -p TMP +rosrun polygon_coverage_solvers GLKH $par $3 +/bin/rm -f $par diff --git a/polygon_coverage_solvers/src/gk_ma.cc b/polygon_coverage_solvers/src/gk_ma.cc deleted file mode 100644 index a5f565c..0000000 --- a/polygon_coverage_solvers/src/gk_ma.cc +++ /dev/null @@ -1,198 +0,0 @@ -/* - * polygon_coverage_planning implements algorithms for coverage planning in - * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous - * Systems Lab, ETH Zürich - * - * This program is free software: you can redistribute it and/or modify it under - * the terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * This program is distributed in the hope that it will be useful, but WITHOUT - * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS - * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more - * details. - * - * You should have received a copy of the GNU General Public License along with - * this program. If not, see . - */ - -#include "polygon_coverage_solvers/gk_ma.h" - -#include -#include - -#include -#include -#include - -namespace polygon_coverage_planning { -namespace gk_ma { - -const std::string kFile = "GkMa.exe"; -const std::string kPackageName = "polygon_coverage_solvers"; -const std::string kPackagePath = ros::package::getPath(kPackageName); -const std::string kCatkinPath = - kPackagePath.substr(0, kPackagePath.find("/src/")); -const std::string kLibraryPath = kCatkinPath + "/devel/lib"; -const std::string kExecutablePath = kLibraryPath + "/" + kFile; - -bool Task::mIsSquare() const { - for (size_t i = 0; i < m.size(); ++i) { - if (m[i].size() != m.size()) { - return false; - } - } - return true; -} - -bool Task::mIsSymmetric() const { - if (!mIsSquare()) { - return false; - } - for (size_t i = 0; i < m.size(); ++i) { - for (size_t j = 0; j < m[i].size(); ++j) { - if (m[i][j] != m[j][i]) { - return false; - } - } - } - return true; -} - -GkMa::GkMa() { - domain_ = mono_jit_init(kFile.c_str()); - ROS_ASSERT(domain_); - MonoAssembly* assembly = - mono_domain_assembly_open(domain_, kExecutablePath.c_str()); - ROS_ASSERT(assembly); - - // Load OurSolver class. - MonoImage* image = mono_assembly_get_image(assembly); - solver_class_ = mono_class_from_name(image, "GkMa", "OurSolver"); - ROS_ASSERT_MSG(solver_class_, "Cannot find OurSolver in assembly %s", - mono_image_get_filename(image)); - solver_ = mono_object_new(domain_, solver_class_); // Allocate memory. -} - -GkMa::~GkMa() { mono_jit_cleanup(domain_); } - -void GkMa::setSolver(const std::string& file, bool binary) { - void* args[2]; - args[0] = mono_string_new(domain_, file.c_str()); - args[1] = &binary; - - // Find constructor method. - void* iter = NULL; - MonoMethod* ctor = NULL; - while ((ctor = mono_class_get_methods(solver_class_, &iter))) { - if (strcmp(mono_method_get_name(ctor), ".ctor") == 0) { - // Check if the ctor takes two arguments. - MonoMethodSignature* sig = mono_method_signature(ctor); - if (mono_signature_get_param_count(sig) == 2) { - break; - } - } - } - - mono_runtime_invoke(ctor, solver_, args, NULL); - MonoObject* exception = nullptr; - mono_runtime_invoke(ctor, solver_, args, &exception); - if (exception) { - mono_print_unhandled_exception(exception); - } -} - -void GkMa::setSolver(const Task& task) { - void* args[3]; - args[0] = vectorOfVectorToMonoArray(task.m); - args[1] = vectorOfVectorToMonoArray(task.clusters); - bool is_symmetric = task.mIsSymmetric(); - args[2] = &is_symmetric; - - // Find constructor method. - void* iter = NULL; - MonoMethod* ctor = NULL; - while ((ctor = mono_class_get_methods(solver_class_, &iter))) { - if (strcmp(mono_method_get_name(ctor), ".ctor") == 0) { - // Check if the ctor takes two arguments. - MonoMethodSignature* sig = mono_method_signature(ctor); - if (mono_signature_get_param_count(sig) == 3) { - break; - } - } - } - - mono_runtime_invoke(ctor, solver_, args, NULL); -} - -MonoArray* GkMa::vectorOfVectorToMonoArray( - const std::vector>& in) const { - MonoArray* result = - mono_array_new(domain_, mono_get_array_class(), in.size()); - for (size_t i = 0; i < in.size(); ++i) { - MonoArray* row = - mono_array_new(domain_, mono_get_int32_class(), in[i].size()); - for (size_t j = 0; j < in[i].size(); ++j) { - mono_array_set(row, int, j, in[i][j]); - } - mono_array_set(result, MonoArray*, i, row); - } - return result; -} - -bool GkMa::solve() { - // TODO(rikba): Check if solver ctor was called. - if (!solver_) { - ROS_ERROR_STREAM("Solver not set."); - return false; - } - - // Find methods SolutionAtIndex(int index) and Solve(). - void* iter = NULL; - MonoMethod* m = NULL; - MonoMethod* solve = NULL; - MonoMethod* solution_at_index = NULL; - while ((m = mono_class_get_methods(solver_class_, &iter))) { - if (strcmp(mono_method_get_name(m), "Solve") == 0) { - solve = m; - } else if (strcmp(mono_method_get_name(m), "SolutionAtIndex") == 0) { - solution_at_index = m; - } - } - if (solve == NULL || solution_at_index == NULL) { - ROS_ERROR_COND(solve == NULL, "Method Solve() not found."); - ROS_ERROR_COND(solution_at_index == NULL, - "Method SolutionAtIndex(int index) not found."); - return false; - } - - // Solve() - mono_runtime_invoke(solve, solver_, NULL, NULL); - - // Solution(). - MonoProperty* prop = - mono_class_get_property_from_name(solver_class_, "SolutionLength"); - MonoMethod* solution_length = mono_property_get_get_method(prop); - if (solution_length == NULL) { - ROS_ERROR_STREAM("Getter SolutionLength() not found."); - return false; - } - MonoObject* result = - mono_runtime_invoke(solution_length, solver_, NULL, NULL); - int num_nodes = *(int*)mono_object_unbox(result); - - solution_.resize(num_nodes); - for (int i = 0; i < num_nodes; ++i) { - void* args[1]; - args[0] = &i; - MonoObject* node = - mono_runtime_invoke(solution_at_index, solver_, args, NULL); - solution_[i] = *(int*)mono_object_unbox(node); - } - - return true; -} - -} // namespace gk_ma -} // namespace polygon_coverage_planning diff --git a/polygon_coverage_solvers/src/glkh.cc b/polygon_coverage_solvers/src/glkh.cc new file mode 100644 index 0000000..1926aa6 --- /dev/null +++ b/polygon_coverage_solvers/src/glkh.cc @@ -0,0 +1,164 @@ +/* + * polygon_coverage_planning implements algorithms for coverage planning in + * general polygons with holes. Copyright (C) 2019, Rik Bähnemann, Autonomous + * Systems Lab, ETH Zürich + * + * This program is free software: you can redistribute it and/or modify it under + * the terms of the GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) any later + * version. + * + * This program is distributed in the hope that it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS + * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more + * details. + * + * You should have received a copy of the GNU General Public License along with + * this program. If not, see . + */ + +#include "polygon_coverage_solvers/glkh.h" +#include "polygon_coverage_solvers/utils.h" + +#include +#include +#include + +#include +#include +#include + +namespace polygon_coverage_planning { +namespace glkh { + +const std::string GLKH_EXE = "rosrun polygon_coverage_solvers run_glkh.bash"; +const std::filesystem::path TMP_DIR{"/tmp/glkh/"}; +const std::filesystem::path PROBLEM_FILE = TMP_DIR / "sweep_plan.gtsp"; +const std::filesystem::path TOUR_FILE = TMP_DIR / "sweep_plan.tour"; + +bool Task::mIsSquare() const { + for (size_t i = 0; i < m.size(); ++i) { + if (m[i].size() != m.size()) { + return false; + } + } + return true; +} + +bool Task::mIsSymmetric() const { + if (!mIsSquare()) { + return false; + } + for (size_t i = 0; i < m.size(); ++i) { + for (size_t j = 0; j < m[i].size(); ++j) { + if (m[i][j] != m[j][i]) { + return false; + } + } + } + return true; +} + +Glkh::Glkh() {} + +void Glkh::setSolver(const Task& task) { + // Create problem file in GTSPLIB format + // see: http://webhotel4.ruc.dk/~keld/research/GLKH/GLKH_Report.pdf + + std::string name = "sweep_plan"; + std::string type = task.mIsSymmetric() ? "GTSP" : "AGTSP"; + std::string comment = "Created by polygon_coverage_solvers."; + size_t dimension = task.m.size(); + size_t num_sets = task.clusters.size(); + std::string edge_weight_type = "EXPLICIT"; + std::string edge_weight_format = "FULL_MATRIX"; + + std::filesystem::create_directory(TMP_DIR); + std::ofstream f(PROBLEM_FILE); + f << "Name: " << name << std::endl; + f << "Type: " << type << std::endl; + f << "COMMENT: " << comment << std::endl; + f << "DIMENSION: " << dimension << std::endl; + f << "GTSP_SETS: " << num_sets << std::endl; + f << "EDGE_WEIGHT_TYPE: " << edge_weight_type << std::endl; + f << "EDGE_WEIGHT_FORMAT: " << edge_weight_format << std::endl; + + f << "EDGE_WEIGHT_SECTION:" << std::endl; + for (size_t i = 0; i < task.m.size(); i++) + { + for (auto w_ij : task.m[i]) + { + if (w_ij == 2147483647) + { + // TODO: FIX Overflow + w_ij = 9999999; + } + f << w_ij << " "; + } + f << std::endl; + } + + f << "GTSP_SET_SECTION:" << std::endl; + for (size_t i = 0; i < task.clusters.size(); i++) + { + f << i + 1 << " "; + for (auto vertex_idx : task.clusters[i]) + { + f << vertex_idx + 1 << " "; + } + f << -1 << std::endl; + } + + f << "EOF" << std::endl; + f.close(); +} + +bool Glkh::solve() { + std::filesystem::current_path(TMP_DIR); + std::string cmd = GLKH_EXE + " " + PROBLEM_FILE.string() + " " + TOUR_FILE.string(); + bool error = system(cmd.c_str()); + if (error) + { + return false; + } + + // Read solution file from disk + std::ifstream infile(TOUR_FILE); + std::string line; + int dimension = -1; + while (std::getline(infile, line)) + { + if (line != "TOUR_SECTION") + { + std::istringstream iss(line); + std::string key; + if (std::getline(iss, key, ':')) + { + trim(key); + if (key == "DIMENSION") + { + iss >> dimension; + } + } + } + else + { + assert(dimension > -1); + solution_.resize(dimension); + for(int i = 0; i < dimension && std::getline(infile, line); i++) + { + std::istringstream iss(line); + int n; + iss >> n; + solution_[i] = n - 1; + } + assert(std::getline(infile, line) && line == "-1"); + } + } + + infile.close(); + return true; +} + +} // namespace glkh +} // namespace polygon_coverage_planning diff --git a/polygon_coverage_solvers/test/gk_ma-test.cpp b/polygon_coverage_solvers/test/glkh-test.cpp similarity index 63% rename from polygon_coverage_solvers/test/gk_ma-test.cpp rename to polygon_coverage_solvers/test/glkh-test.cpp index 9b85d33..71220a2 100644 --- a/polygon_coverage_solvers/test/gk_ma-test.cpp +++ b/polygon_coverage_solvers/test/glkh-test.cpp @@ -20,19 +20,64 @@ #include #include #include +#include #include #include -#include "polygon_coverage_solvers/gk_ma.h" +#include "polygon_coverage_solvers/glkh.h" + using namespace polygon_coverage_planning; -using namespace gk_ma; +using namespace glkh; const std::string kPackageName = "polygon_coverage_solvers"; -TEST(GkMa, LoadFromFile) { - GkMa& instance = GkMa::getInstance(); +int32_t read_int_from_stream(std::ifstream &stream) +{ + int32_t num; + stream.read((char*)&num, sizeof(num)); + return num; +} + +Task read_binary_gtsp(const std::string& file) { + // Read GTSP file (binary format) + std::ifstream input(file, std::ios::binary); + + int dimension = read_int_from_stream(input); + std::cout << dimension << std::endl; + int num_clusters = read_int_from_stream(input); + // 3 if symmetric and triangle, 2 if asymmetric and triangle, 1 if symmetric and non-triangle and 0 otherwise. + // bool is_symmetric = read_int_from_stream(input) % 2 == 1; + read_int_from_stream(input); + + std::vector> clusters(num_clusters); + for (int i = 0; i < num_clusters; i++) + { + int32_t cluster_size; + cluster_size = read_int_from_stream(input); + clusters[i].resize(cluster_size); + for (int j = 0; j < cluster_size; j++) + { + clusters[i][j] = read_int_from_stream(input); + } + } + + std::vector> m(dimension); + for (int i = 0; i < dimension; i++) + { + m[i].resize(dimension); + for (int j = 0; j < dimension; j++) + { + m[i][j] = read_int_from_stream(input); + } + } + + return Task(m, clusters); +} + +TEST(Glkh, LoadFromFile) { + Glkh& instance = Glkh::getInstance(); // Package directory. std::string instances_path = ros::package::getPath(kPackageName); @@ -46,14 +91,15 @@ TEST(GkMa, LoadFromFile) { "40d198.gtsp", "65rbg323.gtsp"}; for (const std::string& instance_name : instance_names) { std::string file = instances_path + instance_name; - instance.setSolver(file, true); + Task task = read_binary_gtsp(file); + instance.setSolver(task); EXPECT_TRUE(instance.solve()); EXPECT_FALSE(instance.getSolution().empty()); } } -TEST(GkMa, LoadFromTask) { - GkMa& instance = GkMa::getInstance(); +TEST(Glkh, LoadFromTask) { + Glkh& instance = Glkh::getInstance(); std::srand(123456); @@ -61,7 +107,7 @@ TEST(GkMa, LoadFromTask) { for (size_t i = 0; i < m.size(); ++i) { for (size_t j = 0; j < m[i].size(); ++j) { if (i == j) { - m[i][j] = std::numeric_limits::max(); + m[i][j] = std::numeric_limits::max(); // TODO: overflow! } else { m[i][j] = rand() % 100; }