Implement world georeferenciation via UTM zone number

MRPT · Dec 9, 2024 · 50110ac · 50110ac
1 parent 96091c8
commit 50110ac
Show file tree

Hide file tree

Showing 5 changed files with 51 additions and 5 deletions.
diff --git a/docs/sensors.rst b/docs/sensors.rst
@@ -348,6 +348,8 @@ Depth (RGBD) camera
       :language: xml
 
 
+.. _sensors-gps:
+
 GNSS sensor ("GPS")
 ---------------------
 A "GPS sensor" can be attached to a robot with the code shown below. 
@@ -358,7 +360,7 @@ For it to work, the ``world`` XML needs to have a :ref:`georeference tag <world-
 
    .. code-block:: xml
 
-		<include file="../definitions/gnss.sensor.xml"
+		<include file="$(ros2 pkg prefix mvsim)/share/mvsim/definitions/gnss.sensor.xml"
 		  sensor_x="0.0"  sensor_y="0.0" sensor_z="0.50"
 		  sensor_period_sec="1.0"
 		  sensor_name="gps"

diff --git a/docs/world_georeference.rst b/docs/world_georeference.rst
@@ -3,13 +3,13 @@
 Georeference options
 --------------------------------------------
 
-For GNSS ("GPS") sensors to work, they need to know the transformation between
+For :ref:`GNSS sensors <sensors-gps>` ("GPS") to work, they need to know the transformation between
 the simulator world local coordinates and the Earth.
 This can be specified with a ``<georeference>`` tag as documented below.
 
 
 .. code-block:: xml
-   :caption: Georeference example (Using geodetic coordinates)
+   :caption: Georeference example #1: Using geodetic coordinates
 
     <mvsim_world version="1.0">
     ...
@@ -23,9 +23,28 @@ This can be specified with a ``<georeference>`` tag as documented below.
     ...
     </mvsim_world>
 
+.. code-block:: xml
+   :caption: Georeference example #2: Using UTM world coordinates
+
+    <mvsim_world version="1.0">
+    ...
+      <!-- Define georeferenced coordinates to the world so GNSS/GPS sensors can be properly simulated -->
+      <georeference>
+        <utm_zone>30</utm_zone>
+      </georeference>
+    ...
+    </mvsim_world>
+
+
+Parameters for worlds in local coordinates and GNSS geodetic reference:
 
 - ``<latitude>yy</latitude>`` and ``<longitude>xx</longitude>``. Geodetic coordinates of the world (0,0,0) origin.
 
 - ``<height>zz</height>``: The height over the WGS84 ellipsoid for the world (0,0,0).
 
 - ``<world_to_enu_rotation_deg>tt</world_to_enu_rotation_deg>``: An optional rotation (in degrees) if you want North not to be aligned with +Y as it is the default.
+
+
+Parameters for worlds with local coordinates as `UTM coordinates <https://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system>`_:
+
+- ``<utm_zone>nn</utm_zone>``: Specify the UTM zone number (longitude), positive/negative for Northern/Southern Hemisphere.
diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h
@@ -604,11 +604,15 @@ class World : public mrpt::system::COutputLogger
 		 */
 		double world_to_enu_rotation = .0;
 
+		/** UTM zone, positive/negative for Northern/Southern Hemisphere */
+		int utm_zone = 0;
+
 		const TParameterDefinitions params = {
 			{"latitude", {"%lf", &georefCoord.lat.decimal_value}},
 			{"longitude", {"%lf", &georefCoord.lon.decimal_value}},
 			{"height", {"%lf", &georefCoord.height}},
 			{"world_to_enu_rotation_deg", {"%lf_deg", &world_to_enu_rotation}},
+			{"utm_zone", {"%i", &utm_zone}},
 		};
 	};
 

diff --git a/modules/simulator/src/WorldElements/ElevationMap.cpp b/modules/simulator/src/WorldElements/ElevationMap.cpp
@@ -187,7 +187,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node<char>* root)
 		const auto ny = static_cast<unsigned int>(std::ceil((maxy - miny) / resolution_));
 
 		parent()->logFmt(
-			mrpt::system::LVL_INFO,
+			mrpt::system::LVL_DEBUG,
 			"[ElevationMap] Loaded %u points, min_corner=(%lf,%lf), max_corner=(%lf,%lf), "
 			"cells=(%u,%u)",
 			static_cast<unsigned>(data.rows()), minx, miny, maxx, maxy, nx, ny);
@@ -284,7 +284,7 @@ void ElevationMap::loadConfigFrom(const rapidxml::xml_node<char>* root)
 		}
 
 		parent()->logFmt(
-			mrpt::system::LVL_INFO, "[ElevationMap] Applying filtering convolution filter %ux%u",
+			mrpt::system::LVL_DEBUG, "[ElevationMap] Applying filtering convolution filter %ux%u",
 			static_cast<unsigned>(kernel.rows()), static_cast<unsigned>(kernel.cols()));
 
 		elevation_data = applyConvolution(elevation_data, kernel);

diff --git a/modules/simulator/src/World_load_xml.cpp b/modules/simulator/src/World_load_xml.cpp
@@ -11,6 +11,7 @@
 #include <mrpt/core/lock_helper.h>
 #include <mrpt/maps/CSimplePointsMap.h>
 #include <mrpt/system/filesystem.h>	 // extractFileDirectory()
+#include <mrpt/topography/conversions.h>
 #include <mvsim/World.h>
 
 #include <algorithm>  // count()
@@ -232,6 +233,26 @@ void World::parse_tag_lights(const XmlParserContext& ctx)
 void World::parse_tag_georeference(const XmlParserContext& ctx)
 {
 	georeferenceOptions_.parse_from(*ctx.node, *this);
+
+	// handle UTM:
+	auto& g = georeferenceOptions_;
+
+	if (g.utm_zone != 0)
+	{
+		ASSERTMSG_(
+			g.georefCoord.isClear(), "Cannot define both, <utm_zone> and geodetics coordinates");
+
+		// we will use the (lat,lon) of UTM origin as reference for this world:
+		const mrpt::topography::TUTMCoords utmCoords = {0, 0, 0};
+
+		mrpt::topography::UTMToGeodetic(
+			utmCoords, std::abs(g.utm_zone), g.utm_zone < 0 ? 'S' : 'N', g.georefCoord);
+
+		MRPT_LOG_DEBUG_STREAM(
+			"Using UTM georeference: utm_zone="	 //
+			<< g.utm_zone << " geoRef: lat=" << g.georefCoord.lat.getDecimalValue()
+			<< " lon=" << g.georefCoord.lon.getDecimalValue());
+	}
 }
 
 void World::parse_tag_walls(const XmlParserContext& ctx) { process_load_walls(*ctx.node); }