Fix XY flip error

Signed-off-by: Ryan Friedman <[email protected]>

Author: Ryanf55

README.md

@@ -87,3 +87,6 @@ grid_map_geo map_publisher --ros-args \
 # To debug from the launch file
 ros2 launch grid_map_geo load_vrt_launch.xml
 ```
+
+**Note:** `grid_map_geo` uses asserts to catch coding errors; they are enabled by default when 
+building in debug mode, and removed from release mode.

include/grid_map_geo/transform.hpp

@@ -167,7 +167,6 @@ inline Eigen::Vector2d geoToImageNoRot(const std::array<double, 6>& geoTransform
  * @return
  */
 inline bool getGeoCorners(const GDALDatasetUniquePtr& datasetPtr, Corners& corners) {
-  double originX, originY, pixelSizeX, pixelSizeY;
   std::array<double, 6> geoTransform;
 
   // https://gdal.org/tutorials/geotransforms_tut.html#introduction-to-geotransforms

src/grid_map_geo.cpp

@@ -155,8 +155,8 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
     maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
   } else {
     if (maporigin_.position.x() < corners.top_left.x() || maporigin_.position.x() > corners.bottom_right.x() ||
-        maporigin_.position.y() < corners.top_left.y() || maporigin_.position.y() > corners.bottom_left.y()) {
-      std::cerr << "Requested map origin is outside of raster dataset" << std::endl;
+        maporigin_.position.y() < corners.bottom_right.y() || maporigin_.position.y() > corners.top_left.y()) {
+      std::cerr << "The configured map origin is outside of raster dataset!" << std::endl;
       return false;
     }
   }
@@ -173,12 +173,12 @@ bool GridMapGeo::initializeFromGdalDataset(const std::string &path) {
 
   // Compute the center in pixel space, then get the raster bounds nXOff and nYOff to extract with RasterIO
   const auto center_px = geoToImageNoRot(geoTransform, {maporigin_.position.x(), maporigin_.position.y()});
-  const auto nxOff = center_px.x() - grid_width / 2;
-  const auto nYOff = center_px.y() - grid_height / 2;
+  const auto raster_io_x_offset = center_px.x() - grid_width / 2;
+  const auto raster_io_y_offset = center_px.y() - grid_height / 2;
 
   std::vector<float> data(grid_width * grid_height, 0.0f);
-  const auto raster_err = elevationBand->RasterIO(GF_Read, nxOff, nYOff, grid_width, grid_height, &data[0], grid_width,
-                                                  grid_height, GDT_Float32, 0, 0);
+  const auto raster_err = elevationBand->RasterIO(GF_Read, raster_io_x_offset, raster_io_y_offset, grid_width,
+                                                  grid_height, &data[0], grid_width, grid_height, GDT_Float32, 0, 0);
   if (raster_err != CPLE_None) {
     return false;
   }

src/map_publisher.cpp

@@ -82,7 +82,35 @@ class MapPublisher : public rclcpp::Node {
 
     map_ = std::make_shared<GridMapGeo>(frame_id);
     map_->setMaxMapSizePixels(max_map_width, max_map_height);
+
+    rcl_interfaces::msg::ParameterDescriptor origin_descriptor;
+    origin_descriptor.read_only = true;
+    origin_descriptor.description = "Map origin latitude (WGS-84) in degrees.";
+    rcl_interfaces::msg::FloatingPointRange origin_range;
+
+    origin_range.from_value = -90.0;
+    origin_range.to_value = 90.0;
+    origin_descriptor.floating_point_range.push_back(origin_range);
+
+    static_assert(std::numeric_limits<double>::has_quiet_NaN == true, "Need quiet NaN for default value");
+    const auto map_origin_latitude = std::clamp(
+        this->declare_parameter("map_origin_latitude", std::numeric_limits<double>::quiet_NaN(), origin_descriptor),
+        origin_range.from_value, origin_range.to_value);
+
+    origin_range.from_value = -180.0;
+    origin_range.to_value = 180.0;
+    origin_descriptor.floating_point_range.at(0) = origin_range;
+
+    origin_descriptor.description = "Map origin longitude (WGS-84) in degrees.";
+    const auto map_origin_longitude = std::clamp(
+        this->declare_parameter("map_origin_longitude", std::numeric_limits<double>::quiet_NaN(), origin_descriptor),
+        origin_range.from_value, origin_range.to_value);
+
+    map_ = std::make_shared<GridMapGeo>();
+    map_->setMaxMapSizePixels(max_map_width, max_map_height);
+    map_->setGlobalOrigin(ESPG::WGS84, Eigen::Vector3d(map_origin_longitude, map_origin_latitude, 0.0));
     map_->Load(gdal_dataset_path, color_path);
+
     auto timer_callback = [this]() -> void {
       auto msg = grid_map::GridMapRosConverter::toMessage(map_->getGridMap());
       if (msg) {