Added debug stuff to obtain the measurement residual

Signed-off-by: TaikiYamada4 <[email protected]>
TaikiYamada4 · Oct 26, 2023 · 648f510 · 648f510
1 parent 801fd0d
commit 648f510
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 0 deletions.
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp
@@ -130,6 +130,12 @@ class EKFLocalizer : public rclcpp::Node
   //!< @brief diagnostics publisher
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_diag_;
   //!< @brief initial pose subscriber
+
+    //!< @brief measurement residual publisher
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_measurement_residual_pose_;
+  //!< @brief predicted pose publisher
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_predicted_pose_;
+
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
   //!< @brief measurement pose with covariance subscriber
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;
@@ -197,6 +203,8 @@ class EKFLocalizer : public rclcpp::Node
   std::array<double, 36ul> current_pose_covariance_;
   std::array<double, 36ul> current_twist_covariance_;
 
+  geometry_msgs::msg::Pose predicted_ekf_pose_;  //!< @brief pose on prediction stage (debug)
+
   /**
    * @brief computes update & prediction of EKF for each ekf_dt_[s] time
    */

diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp
@@ -89,6 +89,10 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti
   pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "ekf_biased_pose_with_covariance", 1);
   pub_diag_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
+
+  pub_measurement_residual_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("measurement_residual_pose", 1);
+  pub_predicted_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_prediction_pose", 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>(
@@ -201,6 +205,29 @@ void EKFLocalizer::timerCallback()
       if (is_updated) {
         pose_is_updated = true;
       }
+
+      // Get measurement_residual 
+      int delay_step = std::roundf(pose_delay_time_ / ekf_dt_);
+      if(delay_step >= 0 && delay_step < params_.extend_state_step){
+        predicted_ekf_pose_.position.x = ekf_.getXelement(delay_step * dim_x_ + IDX::X);
+        predicted_ekf_pose_.position.y = ekf_.getXelement(delay_step * dim_x_ + IDX::Y);
+        predicted_ekf_pose_.position.z = z_filter_.get_x();
+        predicted_ekf_pose_.orientation =
+          tier4_autoware_utils::createQuaternionFromRPY(roll_filter_.get_x(), pitch_filter_.get_x(), ekf_.getXelement(delay_step * dim_x_ + IDX::YAW));
+
+        // get relative pose with predicted pose
+        geometry_msgs::msg::PoseStamped relative_pose;
+        relative_pose.header.stamp = pose->header.stamp;
+        relative_pose.pose = 
+          tier4_autoware_utils::inverseTransformPose(predicted_ekf_pose_, pose->pose.pose);
+
+        pub_measurement_residual_pose_->publish(relative_pose);
+
+        geometry_msgs::msg::PoseStamped predicted_pose_msg;
+        predicted_pose_msg.header.stamp = this->now();
+        predicted_pose_msg.pose = predicted_ekf_pose_;
+        pub_predicted_pose_->publish(predicted_pose_msg);
+      }
     }
     DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc());
     DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n");