From e48e7ff4579a05c19adf39d541a55511edb7e0d2 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Mon, 5 Sep 2022 20:48:41 +0900 Subject: [PATCH 1/6] fix(ekf_localizer): rename biased yaw topic --- .../pose_estimator/pose_estimator.launch.xml | 2 +- .../pose_twist_fusion_filter.launch.xml | 4 ++-- .../include/ekf_localizer/ekf_localizer.hpp | 4 ++-- .../ekf_localizer/launch/ekf_localizer.launch.xml | 8 ++++---- localization/ekf_localizer/src/ekf_localizer.cpp | 14 +++++++------- 5 files changed, 16 insertions(+), 16 deletions(-) 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/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0c3fa0ae8c3f1..ccbe8c2489411 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -130,7 +130,7 @@ 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_; @@ -203,7 +203,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 w/o yaw bias 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..ba9d7c0096d01 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -89,10 +89,10 @@ 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_biased_pose_ = + create_publisher("ekf_biased_pose", 1); pub_pose_cov_no_yawbias_ = create_publisher( - "ekf_pose_with_covariance_without_yawbias", 1); + "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 +229,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 +661,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; @@ -680,7 +680,7 @@ void EKFLocalizer::publishEstimateResult() 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; + pose_cov_no_yawbias.pose.pose = current_biased_ekf_pose_.pose; pub_pose_cov_no_yawbias_->publish(pose_cov_no_yawbias); /* publish latest twist */ From a61a792358ff46c19cfc3787850d393ddaf0884a Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Mon, 5 Sep 2022 21:00:30 +0900 Subject: [PATCH 2/6] Update topic names and their descriptions in README --- localization/ekf_localizer/README.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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) From 52756aa6f535bb826d8689ef204c58cb36f4dd33 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 5 Sep 2022 12:11:50 +0000 Subject: [PATCH 3/6] ci(pre-commit): autofix --- localization/ekf_localizer/src/ekf_localizer.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index ba9d7c0096d01..43a074964b180 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -89,8 +89,7 @@ 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_biased_pose_ = - create_publisher("ekf_biased_pose", 1); + pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_pose_cov_no_yawbias_ = create_publisher( "ekf_biased_pose_with_covariance", 1); sub_initialpose_ = create_subscription( From fd792c05eec423f2dd6551310842b5b6f47a3449 Mon Sep 17 00:00:00 2001 From: Takeshi Ishita Date: Tue, 6 Sep 2022 13:52:24 +0900 Subject: [PATCH 4/6] Fix comments Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index ccbe8c2489411..f1283d90c9226 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -203,7 +203,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_biased_ekf_pose_; //!< @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_; From 71f7c37e517e7ae4938af323588142b30ef15c95 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Tue, 6 Sep 2022 14:38:37 +0900 Subject: [PATCH 5/6] Rename variables --- .../include/ekf_localizer/ekf_localizer.hpp | 2 +- localization/ekf_localizer/src/ekf_localizer.cpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index f1283d90c9226..c89ce0a8908e2 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -133,7 +133,7 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr - pub_pose_cov_no_yawbias_; + pub_biased_pose_cov_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 43a074964b180..66833aac86a52 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -89,8 +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_biased_pose_ = create_publisher("ekf_biased_pose", 1); - pub_pose_cov_no_yawbias_ = create_publisher( + 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)); @@ -678,9 +679,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_biased_ekf_pose_.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_); From b10afecc4ce9355139d4df245fbc8b3a17295694 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 6 Sep 2022 05:40:11 +0000 Subject: [PATCH 6/6] ci(pre-commit): autofix --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 3 +-- localization/ekf_localizer/src/ekf_localizer.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index c89ce0a8908e2..508e9740c0141 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -132,8 +132,7 @@ class EKFLocalizer : public rclcpp::Node //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr - pub_biased_pose_cov_; + rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 66833aac86a52..2a2f60383042a 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -89,8 +89,7 @@ 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_biased_pose_ = - create_publisher("ekf_biased_pose", 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(