Merge pull request autowarefoundation#1263 from tier4/chore/gnss_pose…

…r/cherry-pick fix(gnss_poser): cherry pick
badai-nguyen · Apr 18, 2024 · 755e422 · 755e422
2 parents 932579d + 5d38b9a
commit 755e422
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 12 deletions.
diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md
@@ -4,6 +4,11 @@
 
 The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance.
 
+This node subscribes to NavSatFix to publish the pose of **base_link**. The data in NavSatFix represents the antenna's position. Therefore, it performs a coordinate transformation using the tf from `base_link` to the antenna's position. The frame_id of the antenna's position refers to NavSatFix's `header.frame_id`.
+(**Note that `header.frame_id` in NavSatFix indicates the antenna's frame_id, not the Earth or reference ellipsoid.** [See also NavSatFix definition.](https://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/NavSatFix.html))
+
+If the transformation from `base_link` to the antenna cannot be obtained, it outputs the pose of the antenna position without performing coordinate transformation.
+
 ## Inner-workings / Algorithms
 
 ## Inputs / Outputs

diff --git a/sensing/gnss_poser/config/gnss_poser.param.yaml b/sensing/gnss_poser/config/gnss_poser.param.yaml
@@ -2,7 +2,6 @@
   ros__parameters:
     base_frame: base_link
     gnss_base_frame: gnss_base_link
-    gnss_frame: gnss
     map_frame: map
     buff_epoch: 1
     use_gnss_ins_orientation: true

diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp
@@ -89,10 +89,9 @@ class GNSSPoser : public rclcpp::Node
   rclcpp::Publisher<tier4_debug_msgs::msg::BoolStamped>::SharedPtr fixed_pub_;
 
   MapProjectorInfo::Message projector_info_;
-  std::string base_frame_;
-  std::string gnss_frame_;
-  std::string gnss_base_frame_;
-  std::string map_frame_;
+  const std::string base_frame_;
+  const std::string gnss_base_frame_;
+  const std::string map_frame_;
   bool received_map_projector_info_ = false;
   bool use_gnss_ins_orientation_;
 

diff --git a/sensing/gnss_poser/schema/gnss_poser.schema.json b/sensing/gnss_poser/schema/gnss_poser.schema.json
@@ -11,11 +11,6 @@
           "default": "base_link",
           "description": "frame id for base_frame"
         },
-        "gnss_frame": {
-          "type": "string",
-          "default": "gnss",
-          "description": "frame id for gnss_frame"
-        },
         "gnss_base_frame": {
           "type": "string",
           "default": "gnss_base_link",

diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp
@@ -31,7 +31,6 @@ GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options)
   tf2_listener_(tf2_buffer_),
   tf2_broadcaster_(*this),
   base_frame_(declare_parameter<std::string>("base_frame")),
-  gnss_frame_(declare_parameter<std::string>("gnss_frame")),
   gnss_base_frame_(declare_parameter<std::string>("gnss_base_frame")),
   map_frame_(declare_parameter<std::string>("map_frame")),
   use_gnss_ins_orientation_(declare_parameter<bool>("use_gnss_ins_orientation")),
@@ -142,8 +141,9 @@ void GNSSPoser::callbackNavSatFix(
   // get TF from gnss_antenna to base_link
   auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared<geometry_msgs::msg::TransformStamped>();
 
+  const std::string gnss_frame = nav_sat_fix_msg_ptr->header.frame_id;
   getStaticTransform(
-    base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp);
+    gnss_frame, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp);
   tf2::Transform tf_gnss_antenna2base_link{};
   tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link);