diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml index 5d9e5a65cc720..750adca4c2fa0 100644 --- a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index f37a5362f2c23..3c109769e13ba 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -11,8 +11,8 @@ - - + + diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index baa770d7dcf9f..f77e9b263a389 100644 --- a/localization/ekf_localizer/README.md +++ b/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) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0c3fa0ae8c3f1..508e9740c0141 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/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::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_pose_no_yawbias_; + rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr - pub_pose_cov_no_yawbias_; + rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -203,7 +202,7 @@ class EKFLocalizer : public rclcpp::Node std::queue 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 current_pose_covariance_; std::array current_twist_covariance_; diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index c122cdc895d31..9ec05bae8a864 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -29,8 +29,8 @@ - - + + @@ -66,8 +66,8 @@ - - + + diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 00f6b760efaef..2a2f60383042a 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/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( "ekf_twist_with_covariance", 1); pub_yaw_bias_ = create_publisher("estimated_yaw_bias", 1); - pub_pose_no_yawbias_ = - create_publisher("ekf_pose_without_yawbias", 1); - pub_pose_cov_no_yawbias_ = create_publisher( - "ekf_pose_with_covariance_without_yawbias", 1); + pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); + pub_biased_pose_cov_ = create_publisher( + "ekf_biased_pose_with_covariance", 1); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -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_);