mrpt_map_server now publishes the map georeferenciation metadata, as …

…topics and /tf (frames: utm, enu)

mrpt_map_server/CMakeLists.txt

@@ -8,9 +8,13 @@ find_package(rclcpp_components REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(mrpt_msgs REQUIRED)
 find_package(mrpt_nav_interfaces REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 find_package(mrpt-maps REQUIRED)
 find_package(mrpt-ros2bridge REQUIRED)
+find_package(mrpt-topography REQUIRED)  # shipped by ROS pkg mrpt_libobs
 find_package(mp2p_icp_map REQUIRED)
 
 message(STATUS "MRPT_VERSION: ${MRPT_VERSION}")
@@ -56,6 +60,7 @@ target_include_directories(map_server_node
 target_link_libraries(map_server_node
   mrpt::maps
   mrpt::ros2bridge
+  mrpt::topography
   mola::mp2p_icp_map
 )
 
@@ -66,6 +71,9 @@ ament_target_dependencies(
   nav_msgs
   mrpt_msgs
   mrpt_nav_interfaces
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 #############

mrpt_map_server/README.md

@@ -64,6 +64,7 @@ None.
 ### Published topics
 * ``${pub_mm_topic}/metric_map`` (Default: ``mrpt_map/metric_map``) (``mrpt_msgs::msg::GenericObject``) (topic name can be changed with parameter `pub_mm_topic`).
 * ``${pub_mm_topic}/geo_ref`` (Default: ``mrpt_map/geo_ref``) (``mrpt_msgs::msg::GenericObject``). An MRPT-serialization of ``mp2p_icp::metric_map_t::Georeferencing`` metadata (topic name can be changed with parameter `pub_mm_topic`).
+* ``${pub_mm_topic}/geo_ref_metadata`` (Default: ``mrpt_map/geo_ref_metadata``)(``mrpt_nav_interfaces::msgs::msg::GeoreferencingMetadata``). A ROS plain message with the contents of ``mp2p_icp::metric_map_t::Georeferencing`` metadata.
 * ``${pub_mm_topic}/<LAYER_NAME>`` (Default: ``mrpt_map/<LAYER_NAME>``) (``mrpt_msgs::msg::GenericObject``) 
 * ``${pub_mm_topic}/<LAYER_NAME>_points`` (``sensor_msgs::msg::PointCloud2``), one per map layer.
 * ``${pub_mm_topic}/<LAYER_NAME>_gridmap`` (``nav_msgs::msg::OccupancyGrid``)
@@ -126,5 +127,5 @@ Launch a map server from a custom `.mm` map ([launch file](../mrpt_tutorials/lau
 which in the launch file is read from the environment variable `MM_FILE`, so it can be used like:
  
 ```bash
-MM_FILE=/path/to/my/map.mm ros2 launch mrpt_tutorials demo_map_server_from_mm.launch.py
+ros2 launch mrpt_tutorials demo_map_server_from_mm.launch.py mm_file:=/path/to/my/map.mm
 ```

mrpt_map_server/include/mrpt_map_server/map_server_node.h

@@ -11,16 +11,18 @@
 #include <mp2p_icp/metricmap.h>
 #include <mrpt/config/CConfigFile.h>
 #include <mrpt/maps/CMultiMetricMap.h>
-
-#include "mrpt_msgs/msg/generic_object.hpp"
-#include "mrpt_nav_interfaces/srv/get_gridmap_layer.hpp"
-#include "mrpt_nav_interfaces/srv/get_layers.hpp"
-#include "mrpt_nav_interfaces/srv/get_pointmap_layer.hpp"
-#include "nav_msgs/msg/map_meta_data.hpp"
-#include "nav_msgs/msg/occupancy_grid.hpp"
-#include "nav_msgs/srv/get_map.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <tf2_ros/static_transform_broadcaster.h>
+
+#include <mrpt_msgs/msg/generic_object.hpp>
+#include <mrpt_nav_interfaces/msg/georeferencing_metadata.hpp>
+#include <mrpt_nav_interfaces/srv/get_gridmap_layer.hpp>
+#include <mrpt_nav_interfaces/srv/get_layers.hpp>
+#include <mrpt_nav_interfaces/srv/get_pointmap_layer.hpp>
+#include <nav_msgs/msg/map_meta_data.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/srv/get_map.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
 
 class MapServer : public rclcpp::Node
 {
@@ -38,8 +40,7 @@ class MapServer : public rclcpp::Node
 
 	std::string frame_id_ = "map";
 
-	rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr m_service_map;  //!< service for map server
-	nav_msgs::srv::GetMap::Response m_response_ros;	 //!< response from the map server
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
 
 	/// metric map: will be used whatever is the incoming map format.
 	mp2p_icp::metric_map_t theMap_;
@@ -84,6 +85,9 @@ class MapServer : public rclcpp::Node
 	// for the top-level mm metric map:
 	PerTopicData<mrpt_msgs::msg::GenericObject> pubMM_;
 
+	PerTopicData<mrpt_msgs::msg::GenericObject> pubGeoRef_;
+	PerTopicData<mrpt_nav_interfaces::msg::GeoreferencingMetadata> pubGeoRefMsg_;
+
 	// clang-format off
 	// Binary form of each layer:
 	std::map<std::string, PerTopicData<mrpt_msgs::msg::GenericObject>> pubLayers_;

mrpt_map_server/package.xml

@@ -14,11 +14,15 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>mp2p_icp</depend>
+  <depend>mrpt_libobs</depend>
   <depend>mrpt_libmaps</depend>
   <depend>mrpt_libros_bridge</depend>
   <depend>mrpt_msgs</depend>
   <depend>mrpt_nav_interfaces</depend>
   <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
 
   <depend>ament_lint_common</depend>
   <depend>ament_lint_auto</depend>

mrpt_map_server/src/map_server_node.cpp

@@ -6,8 +6,7 @@
    | All rights reserved. Released under BSD 3-Clause license. See LICENSE  |
    +------------------------------------------------------------------------+ */
 
-#include "mrpt_map_server/map_server_node.h"
-
+#include <geometry_msgs/msg/transform_stamped.h>
 #include <mrpt/config/CConfigFile.h>
 #include <mrpt/core/lock_helper.h>
 #include <mrpt/io/CFileGZInputStream.h>
@@ -20,11 +19,15 @@
 #include <mrpt/maps/CVoxelMap.h>
 #include <mrpt/ros2bridge/map.h>
 #include <mrpt/ros2bridge/point_cloud2.h>
+#include <mrpt/ros2bridge/pose.h>
 #include <mrpt/ros2bridge/time.h>
 #include <mrpt/serialization/CArchive.h>
 #include <mrpt/system/filesystem.h>	 // ASSERT_FILE_EXISTS_()
+#include <mrpt/topography/conversions.h>
+#include <mrpt_map_server/map_server_node.h>
 
 #include <nav_msgs/msg/occupancy_grid.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 using namespace mrpt::config;
 using mrpt::maps::CMultiMetricMap;
@@ -132,6 +135,9 @@ void MapServer::init()
 			const mrpt_nav_interfaces::srv::GetPointmapLayer::Request::SharedPtr req,
 			mrpt_nav_interfaces::srv::GetPointmapLayer::Response::SharedPtr res)
 		{ srv_get_pointmap(req, res); });
+
+	// Create TF broadcaster:
+	tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*this);
 }
 
 void MapServer::publish_map()
@@ -237,6 +243,92 @@ void MapServer::publish_map()
 		}
 
 	}  // end for each layer
+
+	// Publish the geo-referenced metadata:
+	// 1/2: as mrpt serialized object
+	{
+		// If it's nullopt, it will be serialized correctly as such.
+		mrpt::io::CMemoryStream memBuf;
+		auto arch = mrpt::serialization::archiveFrom(memBuf);
+
+		arch << theMap_.georeferencing;
+
+		if (!pubGeoRef_.pub)
+		{
+			pubGeoRef_.pub = this->create_publisher<mrpt_msgs::msg::GenericObject>(
+				pub_mm_topic_ + "/geo_ref"s, QoS);
+		}
+
+		mrpt_msgs::msg::GenericObject msg;
+		msg.data.resize(memBuf.getTotalBytesCount());
+		::memcpy(msg.data.data(), memBuf.getRawBufferData(), msg.data.size());
+		pubGeoRef_.pub->publish(msg);
+	}
+
+	// Now, only if there is valid geo-ref data, publish /tf's for
+	// ENU and UTM:
+	// See: https://docs.mola-slam.org/latest/geo-referencing.html
+
+	mrpt_nav_interfaces::msg::GeoreferencingMetadata geoMsg;
+
+	if (theMap_.georeferencing)
+	{
+		// ENU -> MAP
+		const auto T_enu_to_map = theMap_.georeferencing->T_enu_to_map.mean;
+		geometry_msgs::msg::TransformStamped tfMsg;
+
+		tfMsg.header.stamp = this->get_clock()->now();
+		tfMsg.header.frame_id = "enu";	// parent
+		tfMsg.child_frame_id = "map";  // child
+		tfMsg.transform = tf2::toMsg(mrpt::ros2bridge::toROS_tfTransform(T_enu_to_map));
+		tf_broadcaster_->sendTransform(tfMsg);
+
+		// ENU -> UTM
+		mrpt::topography::TUTMCoords utmCoordsOfENU;
+		int utmZone = 0;
+		char utmBand = 0;
+		mrpt::topography::GeodeticToUTM(
+			theMap_.georeferencing->geo_coord, utmCoordsOfENU, utmZone, utmBand);
+
+		// T_enu_to_utm = - utmCoordsOfENU  (without rotation, both are "ENU")
+		const auto T_enu_to_utm = mrpt::poses::CPose3D::FromTranslation(-utmCoordsOfENU);
+
+		tfMsg.header.frame_id = "enu";	// parent
+		tfMsg.child_frame_id = "utm";  // child
+		tfMsg.transform = tf2::toMsg(mrpt::ros2bridge::toROS_tfTransform(T_enu_to_utm));
+		tf_broadcaster_->sendTransform(tfMsg);
+
+		geoMsg.t_enu_to_map =
+			tf2::toMsg(mrpt::ros2bridge::toROS_Pose(theMap_.georeferencing->T_enu_to_map));
+		geoMsg.t_enu_to_utm = tf2::toMsg(mrpt::ros2bridge::toROS_Pose(T_enu_to_utm));
+		geoMsg.utm_zone = utmZone;
+		geoMsg.utm_band = std::string{utmBand};
+	}
+
+	// 2/2: as ROS msg
+	if (!pubGeoRefMsg_.pub)
+	{
+		pubGeoRefMsg_.pub =
+			this->create_publisher<mrpt_nav_interfaces::msg::GeoreferencingMetadata>(
+				pub_mm_topic_ + "/geo_ref_metadata"s, QoS);
+	}
+
+	if (theMap_.georeferencing)
+	{
+		auto g = *theMap_.georeferencing;
+
+		geoMsg.valid = true;
+
+		geoMsg.latitude = g.geo_coord.lat.decimal_value;
+		geoMsg.longitude = g.geo_coord.lon.decimal_value;
+		geoMsg.height = g.geo_coord.height;
+		// the rest of fields are already filled in above.
+	}
+	else
+	{
+		geoMsg.valid = false;
+	}
+	pubGeoRefMsg_.pub->publish(geoMsg);
 }
 
 void MapServer::loop()