fix(ekf_localizer): rename biased pose topics (tier4#1787)

* fix(ekf_localizer): rename biased pose topics
* Update topic descriptions in README

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com>

launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml

@@ -5,7 +5,7 @@
     <include file="$(find-pkg-share ndt_scan_matcher)/launch/ndt_scan_matcher.launch.xml">
       <arg name="input_map_points_topic" value="/map/pointcloud_map"/>
       <arg name="input/pointcloud" value="/localization/util/downsample/pointcloud"/>
-      <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias"/>
+      <arg name="input_initial_pose_topic" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
 
       <arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
       <arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -11,8 +11,8 @@
       <arg name="output_odom_name" value="kinematic_state"/>
       <arg name="output_pose_name" value="pose"/>
       <arg name="output_pose_with_covariance_name" value="/localization/pose_with_covariance"/>
-      <arg name="output_pose_without_yawbias_name" value="pose_without_yawbias"/>
-      <arg name="output_pose_with_covariance_without_yawbias_name" value="pose_with_covariance_without_yawbias"/>
+      <arg name="output_biased_pose_name" value="biased_pose"/>
+      <arg name="output_biased_pose_with_covariance_name" value="biased_pose_with_covariance"/>
       <arg name="output_twist_name" value="twist"/>
       <arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
       <arg name="proc_stddev_vx_c" value="10.0"/>

localization/ekf_localizer/README.md

@@ -67,13 +67,13 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi
 
   Estimated pose with covariance.
 
-- ekf_pose_with_covariance (geometry_msgs/PoseStamped)
+- ekf_biased_pose (geometry_msgs/PoseStamped)
 
-  Estimated pose without yawbias effect.
+  Estimated pose including the yaw bias
 
-- ekf_pose_with_covariance_without_yawbias (geometry_msgs/PoseWithCovarianceStamped)
+- ekf_biased_pose_with_covariance (geometry_msgs/PoseWithCovarianceStamped)
 
-  Estimated pose with covariance without yawbias effect.
+  Estimated pose with covariance including the yaw bias
 
 - ekf_twist (geometry_msgs/TwistStamped)
 

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -130,10 +130,9 @@ class EKFLocalizer : public rclcpp::Node
   //!< @brief ekf estimated yaw bias publisher
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_yaw_bias_;
   //!< @brief ekf estimated yaw bias publisher
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_no_yawbias_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_;
   //!< @brief ekf estimated yaw bias publisher
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
-    pub_pose_cov_no_yawbias_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_biased_pose_cov_;
   //!< @brief initial pose subscriber
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
   //!< @brief measurement pose with covariance subscriber
@@ -203,7 +202,7 @@ class EKFLocalizer : public rclcpp::Node
   std::queue<PoseInfo> current_pose_info_queue_;      //!< @brief current measured pose
   geometry_msgs::msg::PoseStamped current_ekf_pose_;  //!< @brief current estimated pose
   geometry_msgs::msg::PoseStamped
-    current_ekf_pose_no_yawbias_;  //!< @brief current estimated pose w/o yaw bias
+    current_biased_ekf_pose_;  //!< @brief current estimated pose without yaw bias correction
   geometry_msgs::msg::TwistStamped current_ekf_twist_;  //!< @brief current estimated twist
   std::array<double, 36ul> current_pose_covariance_;
   std::array<double, 36ul> current_twist_covariance_;

localization/ekf_localizer/launch/ekf_localizer.launch.xml

@@ -29,8 +29,8 @@
   <arg name="output_odom_name" default="ekf_odom"/>
   <arg name="output_pose_name" default="ekf_pose"/>
   <arg name="output_pose_with_covariance_name" default="ekf_pose_with_covariance"/>
-  <arg name="output_pose_without_yawbias_name" default="ekf_pose_without_yawbias"/>
-  <arg name="output_pose_with_covariance_without_yawbias_name" default="ekf_pose_with_covariance_without_yawbias"/>
+  <arg name="output_biased_pose_name" default="ekf_biased_pose"/>
+  <arg name="output_biased_pose_with_covariance_name" default="ekf_biased_pose_with_covariance"/>
   <arg name="output_twist_name" default="ekf_twist"/>
   <arg name="output_twist_with_covariance_name" default="ekf_twist_with_covariance"/>
 
@@ -66,8 +66,8 @@
     <remap from="ekf_odom" to="$(var output_odom_name)"/>
     <remap from="ekf_pose" to="$(var output_pose_name)"/>
     <remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)"/>
-    <remap from="ekf_pose_without_yawbias" to="$(var output_pose_without_yawbias_name)"/>
-    <remap from="ekf_pose_with_covariance_without_yawbias" to="$(var output_pose_with_covariance_without_yawbias_name)"/>
+    <remap from="ekf_biased_pose" to="$(var output_biased_pose_name)"/>
+    <remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)"/>
     <remap from="ekf_twist" to="$(var output_twist_name)"/>
     <remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)"/>
   </node>

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -89,10 +89,9 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
   pub_twist_cov_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
     "ekf_twist_with_covariance", 1);
   pub_yaw_bias_ = create_publisher<tier4_debug_msgs::msg::Float64Stamped>("estimated_yaw_bias", 1);
-  pub_pose_no_yawbias_ =
-    create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose_without_yawbias", 1);
-  pub_pose_cov_no_yawbias_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "ekf_pose_with_covariance_without_yawbias", 1);
+  pub_biased_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_biased_pose", 1);
+  pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "ekf_biased_pose_with_covariance", 1);
   sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1));
   sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
@@ -229,8 +228,8 @@ void EKFLocalizer::setCurrentResult()
   current_ekf_pose_.pose.orientation =
     tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);
 
-  current_ekf_pose_no_yawbias_ = current_ekf_pose_;
-  current_ekf_pose_no_yawbias_.pose.orientation =
+  current_biased_ekf_pose_ = current_ekf_pose_;
+  current_biased_ekf_pose_.pose.orientation =
     tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, ekf_.getXelement(IDX::YAW));
 
   current_ekf_twist_.header.frame_id = "base_link";
@@ -661,7 +660,7 @@ void EKFLocalizer::publishEstimateResult()
 
   /* publish latest pose */
   pub_pose_->publish(current_ekf_pose_);
-  pub_pose_no_yawbias_->publish(current_ekf_pose_no_yawbias_);
+  pub_biased_pose_->publish(current_biased_ekf_pose_);
 
   /* publish latest pose with covariance */
   geometry_msgs::msg::PoseWithCovarianceStamped pose_cov;
@@ -679,9 +678,9 @@ void EKFLocalizer::publishEstimateResult()
   pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW);
   pub_pose_cov_->publish(pose_cov);
 
-  geometry_msgs::msg::PoseWithCovarianceStamped pose_cov_no_yawbias = pose_cov;
-  pose_cov_no_yawbias.pose.pose = current_ekf_pose_no_yawbias_.pose;
-  pub_pose_cov_no_yawbias_->publish(pose_cov_no_yawbias);
+  geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;
+  biased_pose_cov.pose.pose = current_biased_ekf_pose_.pose;
+  pub_biased_pose_cov_->publish(biased_pose_cov);
 
   /* publish latest twist */
   pub_twist_->publish(current_ekf_twist_);