From 41246cd9235d29f840c34b331ac270eb01b40866 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 22 Jan 2024 18:23:19 +0900 Subject: [PATCH 1/2] feat(gnss_poser): use header.frame_id as gnss_frame (#6116) * remove gnss_frame param & use header.frame_id to get anntenna's frame_id Signed-off-by: Kento Yabuuchi * update README Signed-off-by: Kento Yabuuchi * style(pre-commit): autofix --------- Signed-off-by: Kento Yabuuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/gnss_poser/README.md | 5 +++++ sensing/gnss_poser/config/gnss_poser.param.yaml | 1 - sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp | 7 +++---- sensing/gnss_poser/schema/gnss_poser.schema.json | 5 ----- sensing/gnss_poser/src/gnss_poser_core.cpp | 4 ++-- 5 files changed, 10 insertions(+), 12 deletions(-) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md index 1ec7dc1f516ad..6799a1f79ae89 100644 --- 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 index 30be98bba8cf1..1c00b43c2066a 100644 --- 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 index 2118d33bc4b30..97ca1d8bffe77 100644 --- 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::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 index 1d63bb2f4850e..6d9bc716e121a 100644 --- 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 index 3a18dca815f12..ec58226273441 100644 --- 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("base_frame")), - gnss_frame_(declare_parameter("gnss_frame")), gnss_base_frame_(declare_parameter("gnss_base_frame")), map_frame_(declare_parameter("map_frame")), use_gnss_ins_orientation_(declare_parameter("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(); + 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); From 5d38b9ae16a8e2b2512f6d9a37743ba319446203 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Tue, 20 Feb 2024 22:20:35 +0900 Subject: [PATCH 2/2] fix(gnss_poser): fix tf transform (#6404) Signed-off-by: Yamato Ando Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- sensing/gnss_poser/src/gnss_poser_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index ec58226273441..61d3863186fda 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -143,7 +143,7 @@ void GNSSPoser::callbackNavSatFix( 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);