[Backport] ESPG to EPSG rename, and enable use of non-CH1903 geotiffs… (#68)

* [Backport] ESPG to EPSG rename, and enable use of non-CH1903 geotiffs (#67)

* Add UTM 32N

* Add offset layer

* Fix format

Author: Jaeyoung-Lim

grid_map_geo/include/grid_map_geo/grid_map_geo.hpp

@@ -42,7 +42,7 @@
 
 #include "transform.hpp"
 struct Location {
-  ESPG espg{ESPG::WGS84};
+  EPSG epsg{EPSG::WGS84};
   Eigen::Vector3d position{Eigen::Vector3d::Zero()};
 };
 
@@ -71,16 +71,16 @@ class GridMapGeo {
    * @param src_coord
    * @param origin
    */
-  void setGlobalOrigin(ESPG src_coord, const Eigen::Vector3d origin);
+  void setGlobalOrigin(EPSG src_coord, const Eigen::Vector3d origin);
 
   /**
    * @brief Get the Global Origin object
    *
    * @param src_coord
    * @param origin
    */
-  void getGlobalOrigin(ESPG& src_coord, Eigen::Vector3d& origin) {
-    src_coord = maporigin_.espg;
+  void getGlobalOrigin(EPSG& src_coord, Eigen::Vector3d& origin) {
+    src_coord = maporigin_.epsg;
     origin = maporigin_.position;
   };
 
@@ -147,6 +147,19 @@ class GridMapGeo {
   bool AddLayerDistanceTransform(const double surface_distance, const std::string& layer_name,
                                  std::string reference_layer = "elevation");
 
+  /**
+   * @brief Add layer with offset
+   *
+   * @param surface_distance surface distance to compute. If smaller than zero, will compute the offset transform
+   * beneath the reference layer
+   * @param layer_name
+   * @return true
+   * @return false
+   */
+
+  bool AddLayerOffsetTransform(const double surface_distance, const std::string& layer_name,
+                               std::string reference_layer = "elevation");
+
   /**
    * @brief Add layer using distance transform
    *

grid_map_geo/include/grid_map_geo/transform.hpp

@@ -36,7 +36,7 @@
 
 #include <Eigen/Dense>
 
-enum class ESPG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21781 };
+enum class EPSG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21781, WGS84_10N = 32610 };
 
 /**
  * @brief Helper function for transforming using gdal
@@ -46,7 +46,7 @@ enum class ESPG { ECEF = 4978, WGS84 = 4326, WGS84_32N = 32632, CH1903_LV03 = 21
  * @param source_coordinates
  * @return Eigen::Vector3d
  */
-Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, const Eigen::Vector3d source_coordinates);
+Eigen::Vector3d transformCoordinates(EPSG src_coord, EPSG tgt_coord, const Eigen::Vector3d source_coordinates);
 
 /**
  * @brief Helper function for transforming using gdal
@@ -56,6 +56,6 @@ Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, const Eigen
  * @param source_coordinates
  * @return Eigen::Vector3d
  */
-Eigen::Vector3d transformCoordinates(ESPG src_coord, const std::string wkt, const Eigen::Vector3d source_coordinates);
+Eigen::Vector3d transformCoordinates(EPSG src_coord, const std::string wkt, const Eigen::Vector3d source_coordinates);
 
 #endif  // GRID_MAP_GEO_TRANSFORM_H

grid_map_geo/src/grid_map_geo.cpp

@@ -118,8 +118,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
 
   double mapcenter_e = originX + pixelSizeX * width * 0.5;
   double mapcenter_n = originY + pixelSizeY * height * 0.5;
-  maporigin_.espg = ESPG::CH1903_LV03;
   maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
+  maporigin_.epsg = static_cast<EPSG>(std::stoi(spatial_ref->GetAttrValue("AUTHORITY", 1)));
 
   Eigen::Vector2d position{Eigen::Vector2d::Zero()};
 
@@ -285,6 +285,19 @@ bool GridMapGeo::AddLayerDistanceTransform(const double surface_distance, const
   return true;
 }
 
+bool GridMapGeo::AddLayerOffsetTransform(const double surface_distance, const std::string &layer_name,
+                                         std::string reference_layer) {
+  grid_map_.add(layer_name);
+
+  for (grid_map::GridMapIterator iterator(grid_map_); !iterator.isPastEnd(); ++iterator) {
+    const grid_map::Index MapIndex = *iterator;
+    Eigen::Vector3d center_pos;
+    grid_map_.getPosition3(reference_layer, MapIndex, center_pos);
+    grid_map_.at(layer_name, MapIndex) = center_pos(2) + surface_distance;
+  }
+  return true;
+}
+
 bool GridMapGeo::AddLayerHorizontalDistanceTransform(const double surface_distance, const std::string &layer_name,
                                                      std::string reference_layer) {
   grid_map_.add(layer_name);
@@ -321,9 +334,9 @@ bool GridMapGeo::AddLayerOffset(const double offset_distance, const std::string
   return true;
 }
 
-void GridMapGeo::setGlobalOrigin(ESPG src_coord, const Eigen::Vector3d origin) {
+void GridMapGeo::setGlobalOrigin(EPSG src_coord, const Eigen::Vector3d origin) {
   // Transform global origin into CH1903 / LV03 coordinates
-  maporigin_.espg = src_coord;
+  maporigin_.epsg = src_coord;
   maporigin_.position = origin;
 }
 

grid_map_geo/src/test_tif_loader.cpp

@@ -72,7 +72,7 @@ class MapPublisher : public rclcpp::Node {
       }
     };
     timer_ = this->create_wall_timer(5s, timer_callback);
-    ESPG epsg;
+    EPSG epsg;
     Eigen::Vector3d map_origin;
     map_->getGlobalOrigin(epsg, map_origin);
 

grid_map_geo/src/transform.cpp

@@ -45,7 +45,7 @@
 #include <gdal/ogr_spatialref.h>
 #endif
 
-Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, const Eigen::Vector3d source_coordinates) {
+Eigen::Vector3d transformCoordinates(EPSG src_coord, EPSG tgt_coord, const Eigen::Vector3d source_coordinates) {
   OGRSpatialReference source, target;
   source.importFromEPSG(static_cast<int>(src_coord));
   target.importFromEPSG(static_cast<int>(tgt_coord));
@@ -61,7 +61,7 @@ Eigen::Vector3d transformCoordinates(ESPG src_coord, ESPG tgt_coord, const Eigen
   return target_coordinates;
 }
 
-Eigen::Vector3d transformCoordinates(ESPG src_coord, const std::string wkt, const Eigen::Vector3d source_coordinates) {
+Eigen::Vector3d transformCoordinates(EPSG src_coord, const std::string wkt, const Eigen::Vector3d source_coordinates) {
   OGRSpatialReference source, target;
   char* wkt_string = const_cast<char*>(wkt.c_str());
   source.importFromEPSG(static_cast<int>(src_coord));

grid_map_geo/test/unit/test_grid_map_geo.cpp

@@ -7,11 +7,11 @@
 TEST(GridMapTest, geoTransform) {
   Eigen::Vector3d berghaus_wgs84(46.7209147, 9.9219592, 488.0);
   Eigen::Vector3d berghaus_lv03(789823.96735451114, 177416.47911055354, 440.3752994351089);
-  Eigen::Vector3d tranformed_lv03 = transformCoordinates(ESPG::WGS84, ESPG::CH1903_LV03, berghaus_wgs84);
+  Eigen::Vector3d tranformed_lv03 = transformCoordinates(EPSG::WGS84, EPSG::CH1903_LV03, berghaus_wgs84);
   EXPECT_NEAR(tranformed_lv03(0), berghaus_lv03(0), 0.0001);
   EXPECT_NEAR(tranformed_lv03(1), berghaus_lv03(1), 0.0001);
   // EXPECT_NEAR(tranformed_lv03(2), berghaus_lv03(2), 0.0001);
-  Eigen::Vector3d tranformed_wgs84 = transformCoordinates(ESPG::CH1903_LV03, ESPG::WGS84, berghaus_lv03);
+  Eigen::Vector3d tranformed_wgs84 = transformCoordinates(EPSG::CH1903_LV03, EPSG::WGS84, berghaus_lv03);
   EXPECT_NEAR(tranformed_wgs84(0), berghaus_wgs84(0), 0.0001);
   EXPECT_NEAR(tranformed_wgs84(1), berghaus_wgs84(1), 0.0001);
   // EXPECT_NEAR(tranformed_wgs84(2), berghaus_wgs84(2), 0.0001);
@@ -25,7 +25,7 @@ TEST(GridMapTest, geoTransform) {
       "PARAMETER[\"rectified_grid_angle\", 90], PARAMETER[\"scale_factor\", 1], PARAMETER[\"false_easting\", 600000], "
       "PARAMETER[\"false_northing\", 200000], UNIT[\"metre\", 1, AUTHORITY[\"EPSG\", \"9001\"]], AXIS[\"Y\", EAST], "
       "AXIS[\"X\", NORTH], AUTHORITY[\"EPSG\", \"21781\"]]";
-  Eigen::Vector3d tranformed_lv032 = transformCoordinates(ESPG::WGS84, wkt, berghaus_wgs84);
+  Eigen::Vector3d tranformed_lv032 = transformCoordinates(EPSG::WGS84, wkt, berghaus_wgs84);
   EXPECT_NEAR(tranformed_lv032(0), berghaus_lv03(0), 0.0001);
   EXPECT_NEAR(tranformed_lv032(1), berghaus_lv03(1), 0.0001);
   // EXPECT_NEAR(tranformed_lv032(2), berghaus_lv03(2), 0.0001);

grid_map_geo_msgs/msg/GeographicMapInfo.msg

@@ -4,7 +4,7 @@
 std_msgs/Header header
 
 
-# ESPG of the coordinate the map is in
+# EPSG of the coordinate the map is in
 uint16 geo_coordinate
 
 # Size of the map in pixels