Merge pull request #149 from mrpt-ros-pkg/feature/utm-coordinates

Support UTM global coordinates for geo-referenciated maps

.github/workflows/build-ros.yml

@@ -45,6 +45,7 @@ jobs:
         uses: ros-tooling/[email protected]
         with:
           required-ros-distributions: ${{ matrix.ros_distribution }}
+          use-ros2-testing: true
 
       - name: Install rosdep dependencies
         run: |

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

@@ -13,12 +13,16 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>mp2p_icp</depend>
+  <depend version_gte="1.6.2">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_cmake_lint_cmake</depend>
   <depend>ament_cmake_xmllint</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()

mrpt_nav_interfaces/CMakeLists.txt

@@ -15,21 +15,24 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(mrpt_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "action/NavigateGoal.action"
-  "action/NavigateWaypoints.action"
-  "msg/NavigationFeedback.msg"
-  "msg/NavigationFinalStatus.msg"
-  "srv/GetLayers.srv"
-  "srv/GetGridmapLayer.srv"
-  "srv/GetPointmapLayer.srv"
-  "srv/MakePlanFromTo.srv"
-  "srv/MakePlanTo.srv"
-  DEPENDENCIES
+  action/NavigateGoal.action
+  action/NavigateWaypoints.action
+  msg/GeoreferencingMetadata.msg
+  msg/NavigationFeedback.msg
+  msg/NavigationFinalStatus.msg
+  srv/GetLayers.srv
+  srv/GetGridmapLayer.srv
+  srv/GetPointmapLayer.srv
+  srv/MakePlanFromTo.srv
+  srv/MakePlanTo.srv
+DEPENDENCIES
   std_msgs
   nav_msgs
   mrpt_msgs
+  geometry_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

mrpt_nav_interfaces/msg/GeoreferencingMetadata.msg

@@ -0,0 +1,18 @@
+# For a diagram of the frames defined in this message, refer to online docs: 
+# https://docs.mola-slam.org/latest/geo-referencing.html
+
+# This is true only if the map is georeferenced. If it is false,
+# the rest of the fields in this message should be ignored.
+bool valid
+
+geometry_msgs/PoseWithCovariance t_enu_to_map
+geometry_msgs/Pose t_enu_to_utm
+
+# The geodetic coordinates of the ENU reference frame:
+float64 latitude
+float64 longitude
+float64 height
+
+# UTM zone, for the ENU point.
+int32 utm_zone
+string utm_band  # just one letter (or empty if valid=false)

mrpt_nav_interfaces/package.xml

@@ -15,6 +15,7 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>action_msgs</depend>
+  <depend>geometry_msgs</depend>
   <depend>mrpt_msgs</depend>
   <depend>nav_msgs</depend>
 

mrpt_tutorials/launch/demo_map_server_from_mm.launch.py

@@ -16,12 +16,16 @@
 def generate_launch_description():
     tutsDir = get_package_share_directory("mrpt_tutorials")
 
+    arg_mm_file = DeclareLaunchArgument(
+        name='mm_file'
+    )
+
     mrpt_map_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([os.path.join(
             get_package_share_directory('mrpt_map_server'), 'launch',
             'mrpt_map_server.launch.py')]),
         launch_arguments={
-            'mm_file': os.environ.get('MM_FILE', 'default.mm'),
+            'mm_file': LaunchConfiguration('mm_file'),
         }.items()
     )
 
@@ -34,5 +38,6 @@ def generate_launch_description():
     )
     return LaunchDescription([
         mrpt_map_launch,
+        arg_mm_file,
         rviz2_node
     ])