diff --git a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index 0a55a5005dde1..febff8c2ee244 100644 --- a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -50,8 +50,8 @@ class PoseInstabilityDetector : public rclcpp::Node }; explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - ThresholdValues calculate_threshold(double interval_sec); - void dead_reckon( + ThresholdValues calculate_threshold(double interval_sec) const; + static void dead_reckon( PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, const std::deque & twist_deque, Pose::SharedPtr & estimated_pose); @@ -60,7 +60,7 @@ class PoseInstabilityDetector : public rclcpp::Node void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); void callback_timer(); - std::deque clip_out_necessary_twist( + static std::deque clip_out_necessary_twist( const std::deque & twist_buffer, const rclcpp::Time & start_time, const rclcpp::Time & end_time); @@ -76,8 +76,6 @@ class PoseInstabilityDetector : public rclcpp::Node // parameters const double timer_period_; // [sec] - ThresholdValues threshold_values_; - const double heading_velocity_maximum_; // [m/s] const double heading_velocity_scale_factor_tolerance_; // [%] diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index fa7b2ecf16562..797f18bba3ea9 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -118,21 +118,21 @@ void PoseInstabilityDetector::callback_timer() prev_pose->header = prev_odometry_->header; prev_pose->pose = prev_odometry_->pose.pose; - Pose::SharedPtr DR_pose = std::make_shared(); - dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, DR_pose); + Pose::SharedPtr dr_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, dr_pose); // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_DR = tier4_autoware_utils::inverseTransformPose(*DR_pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_DR.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_DR.orientation); + const Pose ekf_to_dr = tier4_autoware_utils::inverseTransformPose(*dr_pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_dr.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_dr.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; // publish diff_pose for debug PoseStamped diff_pose; diff_pose.header.stamp = latest_odometry_time; diff_pose.header.frame_id = "base_link"; - diff_pose.pose = ekf_to_DR; + diff_pose.pose = ekf_to_dr; diff_pose_pub_->publish(diff_pose); // publish diagnostics @@ -178,7 +178,7 @@ void PoseInstabilityDetector::callback_timer() } PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( - double interval_sec) + double interval_sec) const { // Calculate maximum longitudinal difference const double longitudinal_difference = @@ -229,7 +229,7 @@ PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_thre const double yaw_difference = roll_difference; // Set thresholds - ThresholdValues result_values; + ThresholdValues result_values{}; result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_;