feat(gnss_poser): use header.frame_id as gnss_frame (autowarefoundati…

…on#6116) * remove gnss_frame param & use header.frame_id to get anntenna's frame_id Signed-off-by: Kento Yabuuchi <[email protected]> * update README Signed-off-by: Kento Yabuuchi <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Kento Yabuuchi <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
kyoichi-sugahara · Jan 23, 2024 · 84305ff · 84305ff
1 parent 490159a
commit 84305ff
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);
+    base_frame_, gnss_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);