diff --git a/module/include/mola_lidar_odometry/LidarOdometry.h b/module/include/mola_lidar_odometry/LidarOdometry.h index 0ab5790..6f83b16 100644 --- a/module/include/mola_lidar_odometry/LidarOdometry.h +++ b/module/include/mola_lidar_odometry/LidarOdometry.h @@ -408,6 +408,10 @@ class LidarOdometry : public mola::FrontEndBase, /** When publishing pose updates, the vehicle frame name.*/ std::string publish_vehicle_frame = "base_link"; + + std::string georef_map_reference_frame = "map"; + std::string georef_map_utm_frame = "utm"; + std::string georef_map_enu_frame = "enu"; }; /** Algorithm parameters */ @@ -493,6 +497,8 @@ class LidarOdometry : public mola::FrontEndBase, #endif void onExposeParameters(); // called after initialization + void publishMetricMapGeoreferencingData(); + private: struct ICP_Input { diff --git a/module/src/LidarOdometry.cpp b/module/src/LidarOdometry.cpp index 22343ce..82f1052 100644 --- a/module/src/LidarOdometry.cpp +++ b/module/src/LidarOdometry.cpp @@ -63,6 +63,7 @@ #include #include #include +#include // GUI: #include @@ -314,6 +315,10 @@ void LidarOdometry::initialize_frontend(const Yaml & c) YAML_LOAD_OPT(params_, publish_reference_frame, std::string); YAML_LOAD_OPT(params_, publish_vehicle_frame, std::string); + YAML_LOAD_OPT(params_, georef_map_reference_frame, std::string); + YAML_LOAD_OPT(params_, georef_map_utm_frame, std::string); + YAML_LOAD_OPT(params_, georef_map_enu_frame, std::string); + if (cfg.has("adaptive_threshold")) params_.adaptive_threshold.initialize(cfg["adaptive_threshold"]); @@ -473,6 +478,8 @@ void LidarOdometry::initialize_frontend(const Yaml & c) bool loadOk = state_.local_map->load_from_file(params_.local_map_updates.load_existing_local_map); ASSERT_(loadOk); + // Publish geo-referenced data for the map, if applicable. + publishMetricMapGeoreferencingData(); } if (!params_.simplemap.load_existing_simple_map.empty()) { @@ -2378,6 +2385,9 @@ MapServer::ReturnStatus LidarOdometry::map_load(const std::string & path) ret.error_message = "Warning: no simplemap (keyframes) file found (expected: '"s + smFile + "'). Required for multisession mapping. "; + // Publish geo-referenced data for the map, if applicable. + if (mmLoadOk) publishMetricMapGeoreferencingData(); + if (ret.success) { state_.local_map_needs_viz_update = true; // refresh map in GUI, if enabled @@ -2506,3 +2516,59 @@ void LidarOdometry::onExposeParameters() this->exposeParameters(nv); #endif } + +void LidarOdometry::publishMetricMapGeoreferencingData() +{ +#if MOLA_VERSION_CHECK(1, 6, 0) // we need lu.child_frame + + ASSERT_(state_.local_map); + //TODO: Define new mola_kernel API for sending out geo-ref data and publish as mrpt_nav_interfaces::msg::GeoreferencingMetadata. + + if (!state_.local_map->georeferencing.has_value()) return; // no geo-ref data + + const auto & g = state_.local_map->georeferencing.value(); + + MRPT_LOG_INFO_STREAM( + "Publishing map georeferencing metadata: T_enu_to_map=" + << g.T_enu_to_map.asString() // + << " geo_coord.lat=" << g.geo_coord.lat.getAsString() // + << " geo_coord.lon=" << g.geo_coord.lon.getAsString() // + << " geo_coord.height=" << g.geo_coord.height // + ); + + // ENU -> MAP + { + LocalizationUpdate lu; + + const auto & T_enu_to_map = g.T_enu_to_map.mean; + + lu.method = "map_server"; + lu.reference_frame = params_.georef_map_enu_frame; // "enu" + lu.child_frame = params_.georef_map_reference_frame; // "map" + lu.timestamp = mrpt::Clock::now(); // Anything better? + lu.pose = T_enu_to_map.asTPose(); + + advertiseUpdatedLocalization(lu); + } + + // ENU -> UTM + { + LocalizationUpdate lu; + mrpt::topography::TUTMCoords utmCoordsOfENU; + int utmZone = 0; + char utmBand = 0; + mrpt::topography::GeodeticToUTM(g.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); + + lu.method = "map_server"; + lu.reference_frame = params_.georef_map_enu_frame; // "enu" + lu.child_frame = params_.georef_map_utm_frame; // "utm" + lu.timestamp = mrpt::Clock::now(); // Anything better? + lu.pose = T_enu_to_utm.asTPose(); + + advertiseUpdatedLocalization(lu); + } +#endif +}