Feature/coordinate transformer

ros/src/util/packages/map_tools/CMakeLists.txt

@@ -1,7 +1,10 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(map_tools)
 
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+
 find_package(PCL REQUIRED)
+find_package(PROJ REQUIRED)
 
 find_package(catkin REQUIRED COMPONENTS
         roscpp
@@ -22,8 +25,11 @@ catkin_package(
 
 SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}")
 
-include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} ${PROJ_INCLUDE_DIR})
 
+add_executable(coodinate_test nodes/coordinate_transformer/coodinate_test.cpp nodes/coordinate_transformer/mgrs_converter.cpp)
+add_executable(pcd_latlon2mgrs nodes/coordinate_transformer/pcd_latlon2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp)
+add_executable(pcd_jpxy2mgrs nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp)
 add_executable(pcd_filter nodes/pcd_filter/pcd_filter.cpp)
 add_executable(pcd_binarizer nodes/pcd_binarizer/pcd_binarizer.cpp)
 add_executable(pcd_arealist nodes/pcd_arealist/pcd_arealist.cpp)
@@ -32,6 +38,9 @@ add_executable(pcd2csv nodes/pcd_converter/pcd2csv.cpp)
 add_executable(map_extender nodes/map_extender/map_extender.cpp)
 add_executable(pcd_grid_divider nodes/pcd_grid_divider/pcd_grid_divider.cpp)
 
+target_link_libraries(coodinate_test ${catkin_LIBRARIES} ${PROJ_LIBRARIES})
+target_link_libraries(pcd_latlon2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARIES})
+target_link_libraries(pcd_jpxy2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARIES})
 target_link_libraries(pcd_filter ${catkin_LIBRARIES})
 target_link_libraries(pcd_binarizer ${catkin_LIBRARIES})
 target_link_libraries(pcd_arealist ${catkin_LIBRARIES})
@@ -41,7 +50,7 @@ target_link_libraries(map_extender ${catkin_LIBRARIES})
 target_link_libraries(pcd_grid_divider ${catkin_LIBRARIES})
 
 
-install(TARGETS pcd_filter pcd_binarizer pcd_arealist csv2pcd pcd2csv map_extender pcd_grid_divider
+install(TARGETS coodinate_test pcd_latlon2mgrs pcd_jpxy2mgrs pcd_filter pcd_binarizer pcd_arealist csv2pcd pcd2csv map_extender pcd_grid_divider
         ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
         LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

ros/src/util/packages/map_tools/cmake/FindPROJ.cmake

@@ -0,0 +1,73 @@
+# Find Proj
+# ~~~~~~~~~
+# Copyright (c) 2007, Martin Dobias <wonder.sk at gmail.com>
+# Redistribution and use is allowed according to the terms of the BSD license.
+# For details see the accompanying COPYING-CMAKE-SCRIPTS file.
+#
+# CMake module to search for Proj library
+#
+# If it's found it sets PROJ_FOUND to TRUE
+# and following variables are set:
+#    PROJ_INCLUDE_DIR
+#    PROJ_LIBRARIES
+
+# FIND_PATH and FIND_LIBRARY normally search standard locations
+# before the specified paths. To search non-standard paths first,
+# FIND_* is invoked first with specified paths and NO_DEFAULT_PATH
+# and then again with no specified paths to search the default
+# locations. When an earlier FIND_* succeeds, subsequent FIND_*s
+# searching for the same item do nothing.
+
+# try to use framework on mac
+# want clean framework path, not unix compatibility path
+IF (APPLE)
+  IF (CMAKE_FIND_FRAMEWORK MATCHES "FIRST"
+      OR CMAKE_FRAMEWORK_PATH MATCHES "ONLY"
+      OR NOT CMAKE_FIND_FRAMEWORK)
+    SET (CMAKE_FIND_FRAMEWORK_save ${CMAKE_FIND_FRAMEWORK} CACHE STRING "" FORCE)
+    SET (CMAKE_FIND_FRAMEWORK "ONLY" CACHE STRING "" FORCE)
+    #FIND_PATH(PROJ_INCLUDE_DIR PROJ/proj_api.h)
+    FIND_LIBRARY(PROJ_LIBRARIES PROJ)
+    IF (PROJ_LIBRARIES)
+      # FIND_PATH doesn't add "Headers" for a framework
+      SET (PROJ_INCLUDE_DIR ${PROJ_LIBRARIES}/Headers CACHE PATH "Path to a file.")
+    ENDIF (PROJ_LIBRARIES)
+    SET (CMAKE_FIND_FRAMEWORK ${CMAKE_FIND_FRAMEWORK_save} CACHE STRING "" FORCE)
+  ENDIF ()
+ENDIF (APPLE)
+
+FIND_PATH(PROJ_INCLUDE_DIR proj_api.h
+    "$ENV{LIB_DIR}/include/proj"
+    "$ENV{LIB_DIR}/include"
+    #mingw
+    c:/msys/local/include
+    NO_DEFAULT_PATH
+    )
+FIND_PATH(PROJ_INCLUDE_DIR proj_api.h)
+
+FIND_LIBRARY(PROJ_LIBRARIES NAMES proj PATHS
+    "$ENV{LIB_DIR}/lib"
+    #mingw
+    c:/msys/local/lib
+    NO_DEFAULT_PATH
+    )
+FIND_LIBRARY(PROJ_LIBRARIES NAMES proj)
+
+IF (PROJ_INCLUDE_DIR AND PROJ_LIBRARIES)
+  SET(PROJ_FOUND TRUE)
+ENDIF (PROJ_INCLUDE_DIR AND PROJ_LIBRARIES)
+
+
+IF (PROJ_FOUND)
+
+  IF (NOT PROJ_FIND_QUIETLY)
+    MESSAGE(STATUS "Found Proj: ${PROJ_LIBRARIES}")
+  ENDIF (NOT PROJ_FIND_QUIETLY)
+
+ELSE (PROJ_FOUND)
+
+  IF (PROJ_FIND_REQUIRED)
+    MESSAGE(FATAL_ERROR "Could not find Proj")
+  ENDIF (PROJ_FIND_REQUIRED)
+
+ENDIF (PROJ_FOUND)

ros/src/util/packages/map_tools/nodes/coordinate_transformer/coodinate_test.cpp

@@ -0,0 +1,69 @@
+/*
+ * The rights of this source code conform to
+ * https://github.com/CPFL/Autoware/blob/master/LICENSE
+ *
+ * (latitude, longitude) convert to MGRS
+ * (JP x, y) convert to (latitude, longitude) or MGRS
+ */
+
+#include <iostream>
+#include <string>
+#include <tuple>
+#include <iomanip>
+#include <limits>
+#include "mgrs_converter.hpp"
+
+int main(int argc, char **argv)
+{
+  double lat, lon, alt;
+  double x, y, z;
+  int plane, test;
+  map_tools::MgrsConverter converter;
+
+  std::cout << "choose  1(JP to MGRS) or 2(lat lon to MGRS): ";
+
+  std::cin >> test;
+
+  if (test != 1 && test != 2)
+  {
+    std::cout << "please input 1 or 2" << std::endl;
+    return -1;
+  }
+  else if (test == 1)
+  {
+    std::cout << "input x: ";
+    std::cin >> x;
+    std::cout << "input y: ";
+    std::cin >> y;
+    std::cout << "input z: ";
+    std::cin >> z;
+    std::cout << "input plane: ";
+    std::cin >> plane;
+
+    converter.jpxy2latlon(x, y, z, plane, lat, lon, alt);
+
+    std::cout << "lat, lon, alt: " << std::setprecision(std::numeric_limits<double>::max_digits10) << lat << ", "
+              << std::setprecision(std::numeric_limits<double>::max_digits10) << lon << ", " << alt << std::endl;
+  }
+  else
+  {
+    std::cout << "input latitude: ";
+    std::cin >> lat;
+    std::cout << "input longitude: ";
+    std::cin >> lon;
+  }
+
+  int zone_num = converter.getUtmZoneNumber(lat, lon);
+  std::string zone_letter = converter.getUtmZoneLetter(lat);
+  converter.latlon2utm(lat, lon, x, y);
+
+  std::string mgrs_code;
+  double easting, northing;
+
+  std::tie(mgrs_code, easting, northing) = converter.latlon2mgrs(lat, lon);
+
+  std::cout << "UTM: " << x << ", " << y << ", " << zone_num << ", " << zone_letter << std::endl;
+  std::cout << "MGRS: " << mgrs_code << ", " << easting << ", " << northing << std::endl;
+
+  return 0;
+}