From 7d2e561ecc42c468d7f063620833c94717f64eb9 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Thu, 12 Jan 2023 19:35:40 +0900 Subject: [PATCH 1/6] Remove smoothing_steps from the pose/twist structs --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 2 -- localization/ekf_localizer/src/ekf_localizer.cpp | 8 ++++---- 2 files changed, 4 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 85f7aa8b1829f..56f24f2650e35 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -51,14 +51,12 @@ struct PoseInfo { geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose; int counter; - int smoothing_steps; }; struct TwistInfo { geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist; int counter; - int smoothing_steps; }; class Simple1DFilter diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 528fbdc5344e0..0b36d50463ae7 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -181,7 +181,7 @@ void EKFLocalizer::timerCallback() current_pose_info_queue_.pop(); measurementUpdatePose(*pose_info.pose); ++pose_info.counter; - if (pose_info.counter < pose_info.smoothing_steps) { + if (pose_info.counter < params_.pose_smoothing_steps) { current_pose_info_queue_.push(pose_info); } } @@ -200,7 +200,7 @@ void EKFLocalizer::timerCallback() current_twist_info_queue_.pop(); measurementUpdateTwist(*twist_info.twist); ++twist_info.counter; - if (twist_info.counter < twist_info.smoothing_steps) { + if (twist_info.counter < params_.twist_smoothing_steps) { current_twist_info_queue_.push(twist_info); } } @@ -361,7 +361,7 @@ void EKFLocalizer::callbackPoseWithCovariance( if (!is_activated_) { return; } - PoseInfo pose_info = {msg, 0, params_.pose_smoothing_steps}; + PoseInfo pose_info = {msg, 0}; current_pose_info_queue_.push(pose_info); @@ -374,7 +374,7 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - TwistInfo twist_info = {msg, 0, params_.twist_smoothing_steps}; + TwistInfo twist_info = {msg, 0}; current_twist_info_queue_.push(twist_info); } From 70d1c099cfb5d3a78d610d61afd142a65930bb1c Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Thu, 12 Jan 2023 20:17:43 +0900 Subject: [PATCH 2/6] Remove the PoseInfo and TwistInfo structs --- .../include/ekf_localizer/ekf_localizer.hpp | 21 +++--- .../ekf_localizer/src/ekf_localizer.cpp | 67 +++++++++++-------- 2 files changed, 47 insertions(+), 41 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 56f24f2650e35..0c5da793d0cc7 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -47,17 +47,8 @@ #include #include -struct PoseInfo -{ - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose; - int counter; -}; - -struct TwistInfo -{ - geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr twist; - int counter; -}; +using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; +using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; class Simple1DFilter { @@ -179,8 +170,12 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; /* for model prediction */ - std::queue current_twist_info_queue_; //!< @brief current measured pose - std::queue current_pose_info_queue_; //!< @brief current measured pose + std::queue current_twist_queue_; //!< @brief current measured twist + std::queue current_twist_count_queue_; + + std::queue current_pose_queue_; //!< @brief current measured pose + std::queue current_pose_count_queue_; + geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose geometry_msgs::msg::PoseStamped current_biased_ekf_pose_; //!< @brief current estimated pose without yaw bias correction diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0b36d50463ae7..9d732587a80fb 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -171,18 +171,22 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ - if (!current_pose_info_queue_.empty()) { + if (!current_pose_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); - int pose_info_queue_size = static_cast(current_pose_info_queue_.size()); - for (int i = 0; i < pose_info_queue_size; ++i) { - PoseInfo pose_info = current_pose_info_queue_.front(); - current_pose_info_queue_.pop(); - measurementUpdatePose(*pose_info.pose); - ++pose_info.counter; - if (pose_info.counter < params_.pose_smoothing_steps) { - current_pose_info_queue_.push(pose_info); + for (size_t i = 0; i < current_pose_queue_.size(); ++i) { + const auto pose = current_pose_queue_.front(); + current_pose_queue_.pop(); + + const int count = current_pose_count_queue_.front(); + current_pose_count_queue_.pop(); + + measurementUpdatePose(*pose); + + if (count + 1 < params_.pose_smoothing_steps) { + current_pose_queue_.push(pose); + current_pose_count_queue_.push(count + 1); } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); @@ -190,18 +194,22 @@ void EKFLocalizer::timerCallback() } /* twist measurement update */ - if (!current_twist_info_queue_.empty()) { + if (!current_twist_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); - int twist_info_queue_size = static_cast(current_twist_info_queue_.size()); - for (int i = 0; i < twist_info_queue_size; ++i) { - TwistInfo twist_info = current_twist_info_queue_.front(); - current_twist_info_queue_.pop(); - measurementUpdateTwist(*twist_info.twist); - ++twist_info.counter; - if (twist_info.counter < params_.twist_smoothing_steps) { - current_twist_info_queue_.push(twist_info); + for (size_t i = 0; i < current_twist_queue_.size(); ++i) { + const auto twist = current_twist_queue_.front(); + current_twist_queue_.pop(); + + const int count = current_twist_count_queue_.front(); + current_twist_count_queue_.pop(); + + measurementUpdateTwist(*twist); + + if (count + 1 < params_.twist_smoothing_steps) { + current_twist_queue_.push(twist); + current_twist_count_queue_.push(count + 1); } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); @@ -361,9 +369,9 @@ void EKFLocalizer::callbackPoseWithCovariance( if (!is_activated_) { return; } - PoseInfo pose_info = {msg, 0}; - current_pose_info_queue_.push(pose_info); + current_pose_queue_.push(msg); + current_pose_count_queue_.push(0); updateSimple1DFilters(*msg); } @@ -374,8 +382,8 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - TwistInfo twist_info = {msg, 0}; - current_twist_info_queue_.push(twist_info); + current_twist_queue_.push(msg); + current_twist_count_queue_.push(0); } /* @@ -610,17 +618,17 @@ void EKFLocalizer::publishEstimateResult() pub_odom_->publish(odometry); /* debug measured pose */ - if (!current_pose_info_queue_.empty()) { + if (!current_pose_queue_.empty()) { geometry_msgs::msg::PoseStamped p; - p.pose = current_pose_info_queue_.back().pose->pose.pose; + p.pose = current_pose_queue_.back()->pose.pose; p.header.stamp = current_time; pub_measured_pose_->publish(p); } /* debug publish */ double pose_yaw = 0.0; - if (!current_pose_info_queue_.empty()) { - pose_yaw = tf2::getYaw(current_pose_info_queue_.back().pose->pose.pose.orientation); + if (!current_pose_queue_.empty()) { + pose_yaw = tf2::getYaw(current_pose_queue_.back()->pose.pose.orientation); } tier4_debug_msgs::msg::Float64MultiArrayStamped msg; @@ -677,8 +685,11 @@ void EKFLocalizer::serviceTriggerNode( std_srvs::srv::SetBool::Response::SharedPtr res) { if (req->data) { - while (!current_pose_info_queue_.empty()) current_pose_info_queue_.pop(); - while (!current_twist_info_queue_.empty()) current_twist_info_queue_.pop(); + while (!current_pose_queue_.empty()) current_pose_queue_.pop(); + while (!current_pose_count_queue_.empty()) current_pose_count_queue_.pop(); + + while (!current_twist_queue_.empty()) current_twist_queue_.pop(); + while (!current_twist_count_queue_.empty()) current_twist_count_queue_.pop(); is_activated_ = true; } else { is_activated_ = false; From 0ff4bb0f42348cb4fdc21de87cfc8a560b9c013c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 16 Jan 2023 02:59:54 +0000 Subject: [PATCH 3/6] ci(pre-commit): autofix --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 0c5da793d0cc7..c99fb5bb42fd1 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -170,10 +170,12 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; /* for model prediction */ - std::queue current_twist_queue_; //!< @brief current measured twist + std::queue + current_twist_queue_; //!< @brief current measured twist std::queue current_twist_count_queue_; - std::queue current_pose_queue_; //!< @brief current measured pose + std::queue + current_pose_queue_; //!< @brief current measured pose std::queue current_pose_count_queue_; geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose From 826d20baf18b3911517232c7a33a71ad35864d27 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Mon, 16 Jan 2023 15:16:27 +0900 Subject: [PATCH 4/6] Reduce the scope of type aliases --- .../include/ekf_localizer/ekf_localizer.hpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index c99fb5bb42fd1..550021abe1970 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -47,9 +47,6 @@ #include #include -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; - class Simple1DFilter { public: @@ -106,6 +103,9 @@ class EKFLocalizer : public rclcpp::Node EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); private: + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + const Warning warning_; //!< @brief ekf estimated pose publisher @@ -174,8 +174,7 @@ class EKFLocalizer : public rclcpp::Node current_twist_queue_; //!< @brief current measured twist std::queue current_twist_count_queue_; - std::queue - current_pose_queue_; //!< @brief current measured pose + std::queue current_pose_queue_; //!< @brief current measured pose std::queue current_pose_count_queue_; geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose From 025a04ffa2f96a85cf704d20274f4dd2b718b030 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 16 Jan 2023 06:18:07 +0000 Subject: [PATCH 5/6] ci(pre-commit): autofix --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 3 ++- 1 file changed, 2 insertions(+), 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 550021abe1970..2b3ca7afdca80 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -174,7 +174,8 @@ class EKFLocalizer : public rclcpp::Node current_twist_queue_; //!< @brief current measured twist std::queue current_twist_count_queue_; - std::queue current_pose_queue_; //!< @brief current measured pose + std::queue + current_pose_queue_; //!< @brief current measured pose std::queue current_pose_count_queue_; geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose From 109ffa77c0dd8523f1cfad0f45b88874b4e62238 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Wed, 18 Jan 2023 14:22:27 +0900 Subject: [PATCH 6/6] Revert message type aliases --- .../ekf_localizer/include/ekf_localizer/ekf_localizer.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 2b3ca7afdca80..8ebcc8b7e379f 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -103,9 +103,6 @@ class EKFLocalizer : public rclcpp::Node EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; - const Warning warning_; //!< @brief ekf estimated pose publisher @@ -170,11 +167,11 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; /* for model prediction */ - std::queue + std::queue current_twist_queue_; //!< @brief current measured twist std::queue current_twist_count_queue_; - std::queue + std::queue current_pose_queue_; //!< @brief current measured pose std::queue current_pose_count_queue_;