feat(gnss_poser): subscribe map projector info (#4791)

* feat(gnss_poser): Subscribe map_projector_info Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update readme Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * small fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * update commetn Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * add local cartesian Signed-off-by: kminoda <koji.minoda@tier4.jp> * update launch file Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * create new function for conversion of height Signed-off-by: kminoda <koji.minoda@tier4.jp> * minor update Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * update Signed-off-by: kminoda <koji.minoda@tier4.jp> * rename Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove unnecessary include Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix map origin Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * remove config file Signed-off-by: kminoda <koji.minoda@tier4.jp> * rfix readme Signed-off-by: kminoda <koji.minoda@tier4.jp> --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Sep 1, 2023 · 1628175 · 1628175
1 parent 9eb5b14
commit 1628175
Show file tree

Hide file tree

Showing 9 changed files with 57 additions and 66 deletions.
diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt
@@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
-  config
 )
diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md
@@ -12,6 +12,7 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
 
 | Name                           | Type                                                    | Description                                                                                                                    |
 | ------------------------------ | ------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------ |
+| `/map/map_projector_info`      | `tier4_map_msgs::msg::MapProjectorInfo`                 | map projection info                                                                                                            |
 | `~/input/fix`                  | `sensor_msgs::msg::NavSatFix`                           | gnss status message                                                                                                            |
 | `~/input/autoware_orientation` | `autoware_sensing_msgs::msg::GnssInsOrientationStamped` | orientation [click here for more details](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_sensing_msgs) |
 
@@ -33,7 +34,6 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
 | `gnss_frame`           | string | "gnss"           | frame id                                                                                               |
 | `gnss_base_frame`      | string | "gnss_base_link" | frame id                                                                                               |
 | `map_frame`            | string | "map"            | frame id                                                                                               |
-| `coordinate_system`    | int    | "4"              | coordinate system enumeration; 1: MGRS, 4: UTM Local Coordinate System                                 |
 | `gnss_pose_pub_method` | int    | 0                | 0: Instant Value 1: Average Value 2: Median Value. If 0 is chosen buffer_epoch parameter loses affect. |
 
 ## Assumptions / Known limits

diff --git a/sensing/gnss_poser/config/set_local_origin.param.yaml b/sensing/gnss_poser/config/set_local_origin.param.yaml
diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp
@@ -22,6 +22,7 @@
 #include <geography_utils/height.hpp>
 #include <rclcpp/logging.hpp>
 
+#include <geographic_msgs/msg/geo_point.hpp>
 #include <sensor_msgs/msg/nav_sat_fix.hpp>
 
 #include <string>
@@ -42,24 +43,17 @@ enum class MGRSPrecision {
 
 GNSSStat NavSatFix2UTM(
   const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger,
-  int height_system)
+  const std::string & target_vertical_datum)
 {
   GNSSStat utm;
 
   try {
     GeographicLib::UTMUPS::Forward(
       nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.east_north_up, utm.x,
       utm.y);
-
-    std::string target_height_system;
-    if (height_system == 0) {
-      target_height_system = "EGM2008";
-    } else {
-      target_height_system = "WGS84";
-    }
     utm.z = geography_utils::convert_height(
       nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, "WGS84",
-      target_height_system);
+      target_vertical_datum);
     utm.latitude = nav_sat_fix_msg.latitude;
     utm.longitude = nav_sat_fix_msg.longitude;
     utm.altitude = nav_sat_fix_msg.altitude;
@@ -71,24 +65,20 @@ GNSSStat NavSatFix2UTM(
 
 GNSSStat NavSatFix2LocalCartesianUTM(
   const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
-  sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger, int height_system)
+  const geographic_msgs::msg::GeoPoint geo_point_origin, const rclcpp::Logger & logger,
+  const std::string & target_vertical_datum)
 {
   GNSSStat utm_local;
   try {
     // origin of the local coordinate system in global frame
     GNSSStat utm_origin;
     GeographicLib::UTMUPS::Forward(
-      nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone,
+      geo_point_origin.latitude, geo_point_origin.longitude, utm_origin.zone,
       utm_origin.east_north_up, utm_origin.x, utm_origin.y);
-    std::string target_height_system;
-    if (height_system == 0) {
-      target_height_system = "EGM2008";
-    } else {
-      target_height_system = "WGS84";
-    }
+
     utm_origin.z = geography_utils::convert_height(
-      nav_sat_fix_origin.altitude, nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude,
-      "WGS84", target_height_system);
+      geo_point_origin.altitude, geo_point_origin.latitude, geo_point_origin.longitude, "WGS84",
+      target_vertical_datum);
 
     // individual coordinates of global coordinate system
     double global_x = 0.0;
@@ -104,7 +94,7 @@ GNSSStat NavSatFix2LocalCartesianUTM(
     utm_local.y = global_y - utm_origin.y;
     utm_local.z = geography_utils::convert_height(
                     nav_sat_fix_msg.altitude, nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude,
-                    "WGS84", target_height_system) -
+                    "WGS84", target_vertical_datum) -
                   utm_origin.z;
   } catch (const GeographicLib::GeographicErr & err) {
     RCLCPP_ERROR_STREAM(
@@ -143,9 +133,9 @@ GNSSStat UTM2MGRS(
 
 GNSSStat NavSatFix2MGRS(
   const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision,
-  const rclcpp::Logger & logger, int height_system)
+  const rclcpp::Logger & logger, const std::string & vertical_datum)
 {
-  const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, height_system);
+  const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger, vertical_datum);
   const auto mgrs = UTM2MGRS(utm, precision, logger);
   return mgrs;
 }

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp
@@ -17,6 +17,8 @@
 #include "gnss_poser/convert.hpp"
 #include "gnss_poser/gnss_stat.hpp"
 
+#include <component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_sensing_msgs/msg/gnss_ins_orientation_stamped.hpp>
@@ -48,15 +50,18 @@ class GNSSPoser : public rclcpp::Node
   explicit GNSSPoser(const rclcpp::NodeOptions & node_options);
 
 private:
+  using MapProjectorInfo = map_interface::MapProjectorInfo;
+
+  void callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg);
   void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr);
   void callbackGnssInsOrientationStamped(
     const autoware_sensing_msgs::msg::GnssInsOrientationStamped::ConstSharedPtr msg);
 
   bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg);
   bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg);
   GNSSStat convert(
-    const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system,
-    int height_system);
+    const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
+    const MapProjectorInfo::Message & projector_info);
   geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat);
   geometry_msgs::msg::Point getMedianPosition(
     const boost::circular_buffer<geometry_msgs::msg::Point> & position_buffer);
@@ -81,6 +86,7 @@ class GNSSPoser : public rclcpp::Node
   tf2_ros::TransformListener tf2_listener_;
   tf2_ros::TransformBroadcaster tf2_broadcaster_;
 
+  component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
   rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr nav_sat_fix_sub_;
   rclcpp::Subscription<autoware_sensing_msgs::msg::GnssInsOrientationStamped>::SharedPtr
     autoware_orientation_sub_;
@@ -89,20 +95,18 @@ class GNSSPoser : public rclcpp::Node
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_cov_pub_;
   rclcpp::Publisher<tier4_debug_msgs::msg::BoolStamped>::SharedPtr fixed_pub_;
 
-  CoordinateSystem coordinate_system_;
+  MapProjectorInfo::Message projector_info_;
   std::string base_frame_;
   std::string gnss_frame_;
   std::string gnss_base_frame_;
   std::string map_frame_;
-
-  sensor_msgs::msg::NavSatFix nav_sat_fix_origin_;
+  bool received_map_projector_info_ = false;
   bool use_gnss_ins_orientation_;
 
   boost::circular_buffer<geometry_msgs::msg::Point> position_buffer_;
 
   autoware_sensing_msgs::msg::GnssInsOrientationStamped::SharedPtr
     msg_gnss_ins_orientation_stamped_;
-  int height_system_;
   int gnss_pose_pub_method;
 };
 }  // namespace gnss_poser

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp
@@ -18,8 +18,6 @@
 
 namespace gnss_poser
 {
-enum class CoordinateSystem { MGRS = 1, LOCAL_CARTESIAN_UTM = 4 };
-
 struct GNSSStat
 {
   GNSSStat()

diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml
@@ -2,7 +2,6 @@
 <launch>
   <arg name="input_topic_fix" default="/fix"/>
   <arg name="input_topic_orientation" default="/autoware_orientation"/>
-  <arg name="param_file" default="$(find-pkg-share gnss_poser)/config/set_local_origin.param.yaml"/>
 
   <arg name="output_topic_gnss_pose" default="gnss_pose"/>
   <arg name="output_topic_gnss_pose_cov" default="gnss_pose_cov"/>
@@ -13,10 +12,8 @@
   <arg name="gnss_frame" default="gnss"/>
   <arg name="map_frame" default="map"/>
 
-  <arg name="coordinate_system" default="4" description="1:MGRS, 4:LocalCartesianUTM"/>
   <arg name="buff_epoch" default="1"/>
   <arg name="use_gnss_ins_orientation" default="true"/>
-  <arg name="height_system" default="1" description="0:Orthometric Height 1:Ellipsoid Height"/>
   <arg name="gnss_pose_pub_method" default="0" description="0: Instant Value 1: Average Value 2: Median Value"/>
 
   <node pkg="gnss_poser" exec="gnss_poser" name="gnss_poser" output="screen">
@@ -31,11 +28,8 @@
     <param name="gnss_frame" value="$(var gnss_frame)"/>
     <param name="map_frame" value="$(var map_frame)"/>
 
-    <param name="coordinate_system" value="$(var coordinate_system)"/>
     <param name="buff_epoch" value="$(var buff_epoch)"/>
     <param name="use_gnss_ins_orientation" value="$(var use_gnss_ins_orientation)"/>
-    <param from="$(var param_file)"/>
-    <param name="height_system" value="$(var height_system)"/>
     <param name="gnss_pose_pub_method" value="$(var gnss_pose_pub_method)"/>
   </node>
 </launch>
diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml
@@ -14,6 +14,9 @@
   <build_depend>libboost-dev</build_depend>
 
   <depend>autoware_sensing_msgs</depend>
+  <depend>component_interface_specs</depend>
+  <depend>component_interface_utils</depend>
+  <depend>geographic_msgs</depend>
   <depend>geographiclib</depend>
   <depend>geography_utils</depend>
   <depend>geometry_msgs</depend>

diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp
@@ -34,16 +34,13 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
   use_gnss_ins_orientation_(declare_parameter("use_gnss_ins_orientation", true)),
   msg_gnss_ins_orientation_stamped_(
     std::make_shared<autoware_sensing_msgs::msg::GnssInsOrientationStamped>()),
-  height_system_(declare_parameter<int>("height_system", 1)),
   gnss_pose_pub_method(declare_parameter<int>("gnss_pose_pub_method", 0))
 {
-  int coordinate_system =
-    declare_parameter("coordinate_system", static_cast<int>(CoordinateSystem::MGRS));
-  coordinate_system_ = static_cast<CoordinateSystem>(coordinate_system);
-
-  nav_sat_fix_origin_.latitude = declare_parameter("latitude", 0.0);
-  nav_sat_fix_origin_.longitude = declare_parameter("longitude", 0.0);
-  nav_sat_fix_origin_.altitude = declare_parameter("altitude", 0.0);
+  // Subscribe to map_projector_info topic
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_sub(
+    sub_map_projector_info_,
+    [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { callbackMapProjectorInfo(msg); });
 
   int buff_epoch = declare_parameter("buff_epoch", 1);
   position_buffer_.set_capacity(buff_epoch);
@@ -61,9 +58,24 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
   fixed_pub_ = create_publisher<tier4_debug_msgs::msg::BoolStamped>("gnss_fixed", rclcpp::QoS{1});
 }
 
+void GNSSPoser::callbackMapProjectorInfo(const MapProjectorInfo::Message::ConstSharedPtr msg)
+{
+  projector_info_ = *msg;
+  received_map_projector_info_ = true;
+}
+
 void GNSSPoser::callbackNavSatFix(
   const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr)
 {
+  // Return immediately if map_projector_info has not been received yet.
+  if (!received_map_projector_info_) {
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
+      "map_projector_info has not been received yet. Check if the map_projection_loader is "
+      "successfully launched.");
+    return;
+  }
+
   // check fixed topic
   const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status);
 
@@ -80,8 +92,8 @@ void GNSSPoser::callbackNavSatFix(
     return;
   }
 
-  // get position in coordinate_system
-  const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_, height_system_);
+  // get position
+  const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, projector_info_);
   const auto position = getPosition(gnss_stat);
 
   geometry_msgs::msg::Pose gnss_antenna_pose{};
@@ -186,20 +198,22 @@ bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix
 }
 
 GNSSStat GNSSPoser::convert(
-  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system,
-  int height_system)
+  const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
+  const MapProjectorInfo::Message & map_projector_info)
 {
   GNSSStat gnss_stat;
-  if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) {
+  if (map_projector_info.projector_type == MapProjectorInfo::Message::LOCAL_CARTESIAN_UTM) {
     gnss_stat = NavSatFix2LocalCartesianUTM(
-      nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger(), height_system);
-  } else if (coordinate_system == CoordinateSystem::MGRS) {
+      nav_sat_fix_msg, map_projector_info.map_origin, this->get_logger(),
+      map_projector_info.vertical_datum);
+  } else if (map_projector_info.projector_type == MapProjectorInfo::Message::MGRS) {
     gnss_stat = NavSatFix2MGRS(
-      nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(), height_system);
+      nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger(),
+      map_projector_info.vertical_datum);
   } else {
     RCLCPP_ERROR_STREAM_THROTTLE(
       this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
-      "Unknown Coordinate System");
+      "Unknown Projector type");
   }
   return gnss_stat;
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -37,5 +37,4 @@ rclcpp_components_register_node(gnss_poser_node @@
     ament_auto_package(INSTALL_TO_SHARE
       launch
-      config
     )