fix(ekf_localizer): remove unnecessary publishers (#5423)

Signed-off-by: kminoda <koji.minoda@tier4.jp>
autowarefoundation · Oct 27, 2023 · bfabbca · bfabbca
1 parent e16ebd8
commit bfabbca
Show file tree

Hide file tree

Showing 2 changed files with 0 additions and 29 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -117,10 +117,6 @@ class EKFLocalizer : public rclcpp::Node
   rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_twist_;
   //!< @brief ekf estimated twist with covariance publisher
   rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr pub_twist_cov_;
-  //!< @brief debug info publisher
-  rclcpp::Publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>::SharedPtr pub_debug_;
-  //!< @brief debug measurement pose publisher
-  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_measured_pose_;
   //!< @brief ekf estimated yaw bias publisher
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_yaw_bias_;
   //!< @brief ekf estimated yaw bias publisher

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -109,10 +109,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
   z_filter_.set_proc_dev(1.0);
   roll_filter_.set_proc_dev(0.01);
   pitch_filter_.set_proc_dev(0.01);
-
-  /* debug */
-  pub_debug_ = create_publisher<tier4_debug_msgs::msg::Float64MultiArrayStamped>("debug", 1);
-  pub_measured_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("debug/measured_pose", 1);
 }
 
 /*
@@ -637,27 +633,6 @@ void EKFLocalizer::publishEstimateResult()
   odometry.pose = pose_cov.pose;
   odometry.twist = twist_cov.twist;
   pub_odom_->publish(odometry);
-
-  /* debug measured pose */
-  if (!pose_queue_.empty()) {
-    geometry_msgs::msg::PoseStamped p;
-    p.pose = pose_queue_.back()->pose.pose;
-    p.header.stamp = current_time;
-    pub_measured_pose_->publish(p);
-  }
-
-  /* debug publish */
-  double pose_yaw = 0.0;
-  if (!pose_queue_.empty()) {
-    pose_yaw = tf2::getYaw(pose_queue_.back()->pose.pose.orientation);
-  }
-
-  tier4_debug_msgs::msg::Float64MultiArrayStamped msg;
-  msg.stamp = current_time;
-  msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAW)));   // [0] ekf yaw angle
-  msg.data.push_back(tier4_autoware_utils::rad2deg(pose_yaw));      // [1] measurement yaw angle
-  msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAWB)));  // [2] yaw bias
-  pub_debug_->publish(msg);
 }
 
 void EKFLocalizer::publishDiagnostics()