diff --git a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp index 737c25f8ce024..880ab82418e06 100644 --- a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp @@ -22,11 +22,11 @@ template class AgedObjectQueue { public: - explicit AgedObjectQueue(const int max_age) : max_age_(max_age) {} + explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {} - bool empty() const { return this->size() == 0; } + [[nodiscard]] bool empty() const { return this->size() == 0; } - size_t size() const { return objects_.size(); } + [[nodiscard]] size_t size() const { return objects_.size(); } Object back() const { return objects_.back(); } @@ -39,7 +39,7 @@ class AgedObjectQueue Object pop_increment_age() { const Object object = objects_.front(); - const int age = ages_.front() + 1; + const size_t age = ages_.front() + 1; objects_.pop(); ages_.pop(); @@ -54,13 +54,13 @@ class AgedObjectQueue void clear() { objects_ = std::queue(); - ages_ = std::queue(); + ages_ = std::queue(); } private: - const int max_age_; + const size_t max_age_; std::queue objects_; - std::queue ages_; + std::queue ages_; }; #endif // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp index a4448ecda2f45..713877c1307d2 100644 --- a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp @@ -17,7 +17,7 @@ #include "ekf_localizer/matrix_types.hpp" -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P); -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P); +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P); +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P); #endif // EKF_LOCALIZER__COVARIANCE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp index f4dc6436f6a40..4c92b2947642b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -20,21 +20,21 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold); -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array); #endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 8ed925c1bc228..b78e9b1ee0469 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -58,17 +58,15 @@ class Simple1DFilter x_ = 0; dev_ = 1e9; proc_dev_x_c_ = 0.0; - return; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time time) + void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) { x_ = init_obs; dev_ = obs_dev; latest_time_ = time; initialized_ = true; - return; }; - void update(const double obs, const double obs_dev, const rclcpp::Time time) + void update(const double obs, const double obs_dev, const rclcpp::Time & time) { if (!initialized_) { init(obs, obs_dev, time); @@ -86,10 +84,9 @@ class Simple1DFilter dev_ = (1 - kalman_gain) * dev_; latest_time_ = time; - return; }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } - double get_x() const { return x_; } + [[nodiscard]] double get_x() const { return x_; } private: bool initialized_; @@ -158,10 +155,9 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; /* process noise variance for discrete model */ - double proc_cov_yaw_d_; //!< @brief discrete yaw process noise - double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise - double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 - double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 + double proc_cov_yaw_d_; //!< @brief discrete yaw process noise + double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 + double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 bool is_activated_; @@ -174,44 +170,45 @@ class EKFLocalizer : public rclcpp::Node /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ - void timerCallback(); + void timer_callback(); /** * @brief publish tf for tf_rate [Hz] */ - void timerTFCallback(); + void timer_tf_callback(); /** - * @brief set poseWithCovariance measurement + * @brief set pose with covariance measurement */ - void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** - * @brief set twistWithCovariance measurement + * @brief set twist with covariance measurement */ - void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void callback_twist_with_covariance( + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); /** * @brief set initial_pose to current EKF pose */ - void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** * @brief update predict frequency */ - void updatePredictFrequency(const rclcpp::Time & current_time); + void update_predict_frequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id */ - bool getTransformFromTF( + bool get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); /** * @brief publish current EKF estimation result */ - void publishEstimateResult( + void publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist); @@ -219,23 +216,23 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(const rclcpp::Time & current_time); + void publish_diagnostics(const rclcpp::Time & current_time); /** - * @brief update simple1DFilter + * @brief update simple 1d filter */ - void updateSimple1DFilters( + void update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); /** - * @brief initialize simple1DFilter + * @brief initialize simple 1d filter */ - void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + void init_simple_1d_filters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief trigger node */ - void serviceTriggerNode( + void service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index e88a59ffdfab9..ee360e798f492 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -34,24 +34,13 @@ struct EKFDiagnosticInfo { - EKFDiagnosticInfo() - : no_update_count(0), - queue_size(0), - is_passed_delay_gate(true), - delay_time(0), - delay_time_threshold(0), - is_passed_mahalanobis_gate(true), - mahalanobis_distance(0) - { - } - - size_t no_update_count; - size_t queue_size; - bool is_passed_delay_gate; - double delay_time; - double delay_time_threshold; - bool is_passed_mahalanobis_gate; - double mahalanobis_distance; + size_t no_update_count{0}; + size_t queue_size{0}; + bool is_passed_delay_gate{true}; + double delay_time{0.0}; + double delay_time_threshold{0.0}; + bool is_passed_mahalanobis_gate{true}; + double mahalanobis_distance{0.0}; }; class EKFModule @@ -63,31 +52,32 @@ class EKFModule using Twist = geometry_msgs::msg::TwistStamped; public: - EKFModule(std::shared_ptr warning, const HyperParameters params); + EKFModule(std::shared_ptr warning, const HyperParameters & params); void initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform); - geometry_msgs::msg::PoseStamped getCurrentPose( + [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const; - geometry_msgs::msg::TwistStamped getCurrentTwist(const rclcpp::Time & current_time) const; - double getYawBias() const; - std::array getCurrentPoseCovariance() const; - std::array getCurrentTwistCovariance() const; + [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist( + const rclcpp::Time & current_time) const; + [[nodiscard]] double get_yaw_bias() const; + [[nodiscard]] std::array get_current_pose_covariance() const; + [[nodiscard]] std::array get_current_twist_covariance() const; - size_t find_closest_delay_time_index(double target_value) const; + [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const; void accumulate_delay_time(const double dt); - void predictWithDelay(const double dt); - bool measurementUpdatePose( + void predict_with_delay(const double dt); + bool measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); - bool measurementUpdateTwist( + bool measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); - geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( + geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time); private: diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 56a13d1ecaf55..8d76102e5d64d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -68,14 +68,14 @@ class HyperParameters const double tf_rate_; const bool publish_tf_; const bool enable_yaw_bias_estimation; - const int extend_state_step; + const size_t extend_state_step; const std::string pose_frame_id; const double pose_additional_delay; const double pose_gate_dist; - const int pose_smoothing_steps; + const size_t pose_smoothing_steps; const double twist_additional_delay; const double twist_gate_dist; - const int twist_smoothing_steps; + const size_t twist_smoothing_steps; const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp index f2b9dc626e67a..1f0d75c5958d4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp @@ -18,7 +18,7 @@ #include #include -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); diff --git a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp index 4b2169f1b182f..396aaf7b9a1b4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp @@ -17,11 +17,11 @@ #include -Eigen::Matrix poseMeasurementMatrix(); -Eigen::Matrix twistMeasurementMatrix(); -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix pose_measurement_matrix(); +Eigen::Matrix twist_measurement_matrix(); +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step); -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step); #endif // EKF_LOCALIZER__MEASUREMENT_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp index 6554f80aee014..358a2750bd3a8 100644 --- a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp @@ -19,12 +19,12 @@ #include -inline bool hasInf(const Eigen::MatrixXd & v) +inline bool has_inf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); } -inline bool hasNan(const Eigen::MatrixXd & v) +inline bool has_nan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); } diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp index 85a65828e4ee4..09a87e5fe154b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp @@ -17,10 +17,10 @@ #include "ekf_localizer/matrix_types.hpp" -double normalizeYaw(const double & yaw); -Vector6d predictNextState(const Vector6d & X_curr, const double dt); -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); -Matrix6d processNoiseCovariance( +double normalize_yaw(const double & yaw); +Vector6d predict_next_state(const Vector6d & X_curr, const double dt); +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt); +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); #endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/string.hpp b/localization/ekf_localizer/include/ekf_localizer/string.hpp index a4cd1f320367c..0154e84b88004 100644 --- a/localization/ekf_localizer/include/ekf_localizer/string.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/string.hpp @@ -17,7 +17,7 @@ #include -inline std::string eraseLeadingSlash(const std::string & s) +inline std::string erase_leading_slash(const std::string & s) { std::string a = s; if (a.front() == '/') { diff --git a/localization/ekf_localizer/include/ekf_localizer/warning.hpp b/localization/ekf_localizer/include/ekf_localizer/warning.hpp index a3c8800242e2b..e7456d73ecfdd 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning.hpp @@ -29,7 +29,7 @@ class Warning RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); } - void warnThrottle(const std::string & message, const int duration_milliseconds) const + void warn_throttle(const std::string & message, const int duration_milliseconds) const { RCLCPP_WARN_THROTTLE( node_->get_logger(), *(node_->get_clock()), diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index e1eafdc6f7948..36c0eabd588fa 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,11 +17,12 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); -std::string twistDelayStepWarningMessage( +std::string pose_delay_step_warning_message( const double delay_time, const double delay_time_threshold); -std::string poseDelayTimeWarningMessage(const double delay_time); -std::string twistDelayTimeWarningMessage(const double delay_time); -std::string mahalanobisWarningMessage(const double distance, const double max_distance); +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold); +std::string pose_delay_time_warning_message(const double delay_time); +std::string twist_delay_time_warning_message(const double delay_time); +std::string mahalanobis_warning_message(const double distance, const double max_distance); #endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/src/covariance.cpp b/localization/ekf_localizer/src/covariance.cpp index 0c98ac6857ea5..fc16abf429d93 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -19,9 +19,9 @@ using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::X, IDX::X); @@ -37,9 +37,9 @@ std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) return covariance; } -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX); diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp index 9ff36561abaa9..ecbeaf8b2445a 100644 --- a/localization/ekf_localizer/src/diagnostics.cpp +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -19,7 +19,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated) +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,7 +38,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activ return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) { @@ -69,7 +69,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -85,7 +85,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold) { @@ -112,7 +112,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold) { @@ -141,7 +141,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( // The highest level within the stat_array will be reflected in the merged_stat. // When all stat_array entries are 'OK,' the message of merged_stat will be "OK" -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 687e812679574..8e7121ec73a12 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -34,8 +34,8 @@ #include // clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl // NOLINT +#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} // NOLINT // clang-format on using std::placeholders::_1; @@ -58,12 +58,12 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) /* initialize ros system */ timer_control_ = rclcpp::create_timer( this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), - std::bind(&EKFLocalizer::timerCallback, this)); + std::bind(&EKFLocalizer::timer_callback, this)); if (params_.publish_tf_) { timer_tf_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + std::bind(&EKFLocalizer::timer_tf_callback, this)); } pub_pose_ = create_publisher("ekf_pose", 1); @@ -79,15 +79,17 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); sub_initialpose_ = create_subscription( - "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); + "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( - "in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1)); + "in_pose_with_covariance", 1, + std::bind(&EKFLocalizer::callback_pose_with_covariance, this, _1)); sub_twist_with_cov_ = create_subscription( - "in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1)); + "in_twist_with_covariance", 1, + std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1)); service_trigger_node_ = create_service( "trigger_node_srv", std::bind( - &EKFLocalizer::serviceTriggerNode, this, std::placeholders::_1, std::placeholders::_2), + &EKFLocalizer::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile()); tf_br_ = std::make_shared( @@ -102,9 +104,9 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) } /* - * updatePredictFrequency + * update_predict_frequency */ -void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) +void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time) { if (last_predict_time_) { if (current_time < *last_predict_time_) { @@ -119,7 +121,7 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) ekf_dt_ = 10.0; RCLCPP_WARN( get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); - } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + } else if (ekf_dt_ > static_cast(params_.pose_smoothing_steps) / params_.ekf_rate) { RCLCPP_WARN( get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); } @@ -137,28 +139,28 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) } /* - * timerCallback + * timer_callback */ -void EKFLocalizer::timerCallback() +void EKFLocalizer::timer_callback() { const rclcpp::Time current_time = this->now(); if (!is_activated_) { - warning_->warnThrottle( + warning_->warn_throttle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(current_time); + publish_diagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(current_time); + update_predict_frequency(current_time); /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - ekf_module_->predictWithDelay(ekf_dt_); + ekf_module_->predict_with_delay(ekf_dt_); DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); @@ -177,22 +179,22 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_); if (is_updated) { pose_is_updated = true; // Update Simple 1D filter with considering change of z value due to measurement pose delay const double delay_time = - (t_curr - pose->header.stamp).seconds() + params_.pose_additional_delay; - const auto pose_with_z_delay = ekf_module_->compensatePoseWithZDelay(*pose, delay_time); - updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); + (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; + const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time); + update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps); } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_pose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); @@ -212,16 +214,17 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); + bool is_updated = + ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_); if (is_updated) { twist_is_updated = true; } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_twist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); @@ -230,27 +233,27 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); + ekf_module_->get_current_pose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(current_time); + ekf_module_->get_current_twist(current_time); /* publish ekf result */ - publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(current_time); + publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); + publish_diagnostics(current_time); } /* - * timerTFCallback + * timer_tf_callback */ -void EKFLocalizer::timerTFCallback() +void EKFLocalizer::timer_tf_callback() { if (!is_activated_) { return; } - if (params_.pose_frame_id == "") { + if (params_.pose_frame_id.empty()) { return; } @@ -262,15 +265,15 @@ void EKFLocalizer::timerTFCallback() geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } /* - * getTransformFromTF + * get_transform_from_tf */ -bool EKFLocalizer::getTransformFromTF( +bool EKFLocalizer::get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { @@ -278,8 +281,8 @@ bool EKFLocalizer::getTransformFromTF( tf2_ros::TransformListener tf_listener(tf_buffer); rclcpp::sleep_for(std::chrono::milliseconds(100)); - parent_frame = eraseLeadingSlash(parent_frame); - child_frame = eraseLeadingSlash(child_frame); + parent_frame = erase_leading_slash(parent_frame); + child_frame = erase_leading_slash(child_frame); for (int i = 0; i < 50; ++i) { try { @@ -294,25 +297,25 @@ bool EKFLocalizer::getTransformFromTF( } /* - * callbackInitialPose + * callback_initial_pose */ -void EKFLocalizer::callbackInitialPose( - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) +void EKFLocalizer::callback_initial_pose( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { geometry_msgs::msg::TransformStamped transform; - if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) { + if (!get_transform_from_tf(params_.pose_frame_id, msg->header.frame_id, transform)) { RCLCPP_ERROR( get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", - params_.pose_frame_id.c_str(), initialpose->header.frame_id.c_str()); + params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } - ekf_module_->initialize(*initialpose, transform); - initSimple1DFilters(*initialpose); + ekf_module_->initialize(*msg, transform); + init_simple_1d_filters(*msg); } /* - * callbackPoseWithCovariance + * callback_pose_with_covariance */ -void EKFLocalizer::callbackPoseWithCovariance( +void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { if (!is_activated_) { @@ -323,9 +326,9 @@ void EKFLocalizer::callbackPoseWithCovariance( } /* - * callbackTwistWithCovariance + * callback_twist_with_covariance */ -void EKFLocalizer::callbackTwistWithCovariance( +void EKFLocalizer::callback_twist_with_covariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { // Ignore twist if velocity is too small. @@ -337,9 +340,9 @@ void EKFLocalizer::callbackTwistWithCovariance( } /* - * publishEstimateResult + * publish_estimate_result */ -void EKFLocalizer::publishEstimateResult( +void EKFLocalizer::publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) @@ -353,7 +356,7 @@ void EKFLocalizer::publishEstimateResult( pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; - pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); + pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -368,13 +371,13 @@ void EKFLocalizer::publishEstimateResult( twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; - twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); + twist_cov.twist.covariance = ekf_module_->get_current_twist_covariance(); pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; - yawb.data = ekf_module_->getYawBias(); + yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ @@ -387,38 +390,38 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) +void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; - diag_status_array.push_back(checkProcessActivated(is_activated_)); + diag_status_array.push_back(check_process_activated(is_activated_)); if (is_activated_) { - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, pose_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, params_.pose_gate_dist)); - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn, params_.twist_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("twist", twist_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, twist_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, params_.twist_gate_dist)); } diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status = merge_diagnostic_status(diag_status_array); diag_merged_status.name = "localization: " + std::string(this->get_name()); diag_merged_status.hardware_id = this->get_name(); @@ -428,7 +431,7 @@ void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) pub_diag_->publish(diag_msg); } -void EKFLocalizer::updateSimple1DFilters( +void EKFLocalizer::update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { double z = pose.pose.pose.position.z; @@ -446,7 +449,8 @@ void EKFLocalizer::updateSimple1DFilters( pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); } -void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void EKFLocalizer::init_simple_1d_filters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; @@ -465,7 +469,7 @@ void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovaria /** * @brief trigger node */ -void EKFLocalizer::serviceTriggerNode( +void EKFLocalizer::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { @@ -477,7 +481,6 @@ void EKFLocalizer::serviceTriggerNode( is_activated_ = false; } res->success = true; - return; } #include diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 8977d82f34138..8703d8754eaaf 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -30,58 +30,56 @@ #include // clang-format off -#define DEBUG_PRINT_MAT(X) {\ - if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ -} +#define DEBUG_PRINT_MAT(X) {if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}} // NOLINT // clang-format on -EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) +EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - P(IDX::YAW, IDX::YAW) = 50.0; // for yaw + Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y + p(IDX::YAW, IDX::YAW) = 50.0; // for yaw if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias + p(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias } - P(IDX::VX, IDX::VX) = 1000.0; // for vx - P(IDX::WZ, IDX::WZ) = 50.0; // for wz + p(IDX::VX, IDX::VX) = 1000.0; // for vx + p(IDX::WZ, IDX::WZ) = 50.0; // for wz - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } void EKFModule::initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform) { - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + Eigen::MatrixXd x(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; - X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; - X(IDX::YAW) = + x(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; + x(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; + x(IDX::YAW) = tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - X(IDX::YAWB) = 0.0; - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; + x(IDX::YAWB) = 0.0; + x(IDX::VX) = 0.0; + x(IDX::WZ) = 0.0; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - P(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; + p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; + p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; + p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 0.0001; + p(IDX::YAWB, IDX::YAWB) = 0.0001; } - P(IDX::VX, IDX::VX) = 0.01; - P(IDX::WZ, IDX::WZ) = 0.01; + p(IDX::VX, IDX::VX) = 0.01; + p(IDX::WZ, IDX::WZ) = 0.01; - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } -geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( +geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const { @@ -110,7 +108,8 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( return current_ekf_pose; } -geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & current_time) const +geometry_msgs::msg::TwistStamped EKFModule::get_current_twist( + const rclcpp::Time & current_time) const { const double vx = kalman_filter_.getXelement(IDX::VX); const double wz = kalman_filter_.getXelement(IDX::WZ); @@ -123,17 +122,17 @@ geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & return current_ekf_twist; } -std::array EKFModule::getCurrentPoseCovariance() const +std::array EKFModule::get_current_pose_covariance() const { - return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); } -std::array EKFModule::getCurrentTwistCovariance() const +std::array EKFModule::get_current_twist_covariance() const { - return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_twist_message_covariance(kalman_filter_.getLatestP()); } -double EKFModule::getYawBias() const +double EKFModule::get_yaw_bias() const { return kalman_filter_.getLatestX()(IDX::YAWB); } @@ -153,17 +152,17 @@ size_t EKFModule::find_closest_delay_time_index(double target_value) const // If else, take the closest element. if (lower == accumulated_delay_times_.begin()) { return 0; - } else if (lower == accumulated_delay_times_.end()) { + } + if (lower == accumulated_delay_times_.end()) { return accumulated_delay_times_.size() - 1; - } else { - // Compare the target with the lower bound and the previous element. - auto prev = lower - 1; - bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); - - // Return the index of the closer element. - return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) - : std::distance(accumulated_delay_times_.begin(), lower); } + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); } void EKFModule::accumulate_delay_time(const double dt) @@ -180,69 +179,70 @@ void EKFModule::accumulate_delay_time(const double dt) } } -void EKFModule::predictWithDelay(const double dt) +void EKFModule::predict_with_delay(const double dt) { - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0); const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0); const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0); - const Vector6d X_next = predictNextState(X_curr, dt); - const Matrix6d A = createStateTransitionMatrix(X_curr, dt); - const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); - kalman_filter_.predictWithDelay(X_next, A, Q); + const Vector6d x_next = predict_next_state(x_curr, dt); + const Matrix6d a = create_state_transition_matrix(x_curr, dt); + const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); + kalman_filter_.predictWithDelay(x_next, a, q); } -bool EKFModule::measurementUpdatePose( +bool EKFModule::measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { - warning_->warnThrottle( + warning_->warn_throttle( fmt::format( "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(pose_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + warning_->warn_throttle( + pose_delay_step_warning_message( + pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), 2000); return false; } /* Since the kalman filter cannot handle the rotation angle directly, - offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less - than 2 pi. */ + offset the yaw angle so that the difference from the yaw angle that ekf holds internally + is less than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi + const double yaw_error = normalize_yaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return false; @@ -252,15 +252,15 @@ bool EKFModule::measurementUpdatePose( const Eigen::Vector3d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(0, 0, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance); if (distance > params_.pose_gate_dist) { pose_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.pose_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -268,21 +268,21 @@ bool EKFModule::measurementUpdatePose( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = poseMeasurementMatrix(); - const Eigen::Matrix3d R = - poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); + const Eigen::Matrix c = pose_measurement_matrix(); + const Eigen::Matrix3d r = + pose_measurement_covariance(pose.pose.covariance, params_.pose_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay( +geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time) { const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); @@ -293,34 +293,34 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela return pose_with_z_delay; } -bool EKFModule::measurementUpdateTwist( +bool EKFModule::measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { - warning_->warnThrottle("twist frame_id must be base_link", 2000); + warning_->warn_throttle("twist frame_id must be base_link", 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 2; // vx, wz /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(twist_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - twistDelayStepWarningMessage( + warning_->warn_throttle( + twist_delay_step_warning_message( twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), 2000); return false; @@ -330,7 +330,7 @@ bool EKFModule::measurementUpdateTwist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return false; @@ -339,15 +339,15 @@ bool EKFModule::measurementUpdateTwist( const Eigen::Vector2d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ)); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(4, 4, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance); if (distance > params_.twist_gate_dist) { twist_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.twist_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -355,16 +355,16 @@ bool EKFModule::measurementUpdateTwist( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = twistMeasurementMatrix(); - const Eigen::Matrix2d R = - twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); + const Eigen::Matrix c = twist_measurement_matrix(); + const Eigen::Matrix2d r = + twist_measurement_covariance(twist.twist.covariance, params_.twist_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp index ff5ef13892b73..a71482ab696f6 100644 --- a/localization/ekf_localizer/src/mahalanobis.cpp +++ b/localization/ekf_localizer/src/mahalanobis.cpp @@ -14,7 +14,7 @@ #include "ekf_localizer/mahalanobis.hpp" -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { const Eigen::VectorXd d = x - y; @@ -23,5 +23,5 @@ double squaredMahalanobis( double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { - return std::sqrt(squaredMahalanobis(x, y, C)); + return std::sqrt(squared_mahalanobis(x, y, C)); } diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index 0b3e65bd60f5d..4533bacbee991 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -17,40 +17,40 @@ #include "ekf_localizer/state_index.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" -Eigen::Matrix poseMeasurementMatrix() +Eigen::Matrix pose_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::X) = 1.0; // for pos x + c(1, IDX::Y) = 1.0; // for pos y + c(2, IDX::YAW) = 1.0; // for yaw + return c; } -Eigen::Matrix twistMeasurementMatrix() +Eigen::Matrix twist_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::VX) = 1.0; // for vx - C(1, IDX::WZ) = 1.0; // for wz - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::VX) = 1.0; // for vx + c(1, IDX::WZ) = 1.0; // for wz + return c; } -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { - Eigen::Matrix3d R; + Eigen::Matrix3d r; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), + r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { - Eigen::Matrix2d R; + Eigen::Matrix2d r; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), + r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_YAW); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp index 88fc9f76d7168..22b1f2f8a1c67 100644 --- a/localization/ekf_localizer/src/state_transition.cpp +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -17,7 +17,7 @@ #include -double normalizeYaw(const double & yaw) +double normalize_yaw(const double & yaw) { // FIXME(IshitaTakeshi) I think the computation here can be simplified // FIXME(IshitaTakeshi) Rename the function. This is not normalization @@ -36,7 +36,7 @@ double normalizeYaw(const double & yaw) * (b_k : yaw_bias_k) */ -Vector6d predictNextState(const Vector6d & X_curr, const double dt) +Vector6d predict_next_state(const Vector6d & X_curr, const double dt) { const double x = X_curr(IDX::X); const double y = X_curr(IDX::Y); @@ -45,14 +45,14 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) const double vx = X_curr(IDX::VX); const double wz = X_curr(IDX::WZ); - Vector6d X_next; - X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt); // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - return X_next; + Vector6d x_next; + x_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + x_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + x_next(IDX::YAW) = normalize_yaw(yaw + wz * dt); // dyaw = omega + omega_bias + x_next(IDX::YAWB) = yaw_bias; + x_next(IDX::VX) = vx; + x_next(IDX::WZ) = wz; + return x_next; } /* == Linearized model == @@ -64,32 +64,32 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) * [ 0, 0, 0, 0, 1, 0] * [ 0, 0, 0, 0, 0, 1] */ -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt) +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt) { const double yaw = X_curr(IDX::YAW); const double yaw_bias = X_curr(IDX::YAWB); const double vx = X_curr(IDX::VX); - Matrix6d A = Matrix6d::Identity(); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - return A; + Matrix6d a = Matrix6d::Identity(); + a(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + a(IDX::YAW, IDX::WZ) = dt; + return a; } -Matrix6d processNoiseCovariance( +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) { - Matrix6d Q = Matrix6d::Zero(); - Q(IDX::X, IDX::X) = 0.0; - Q(IDX::Y, IDX::Y) = 0.0; - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw - Q(IDX::YAWB, IDX::YAWB) = 0.0; - Q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz - return Q; + Matrix6d q = Matrix6d::Zero(); + q(IDX::X, IDX::X) = 0.0; + q(IDX::Y, IDX::Y) = 0.0; + q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw + q(IDX::YAWB, IDX::YAWB) = 0.0; + q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx + q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz + return q; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index c69f30b2d6601..ae74c6bfb5fbc 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,7 +18,8 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string pose_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " @@ -26,7 +27,8 @@ std::string poseDelayStepWarningMessage(const double delay_time, const double de return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " @@ -34,19 +36,19 @@ std::string twistDelayStepWarningMessage(const double delay_time, const double d return fmt::format(s, delay_time, delay_time_threshold); } -std::string poseDelayTimeWarningMessage(const double delay_time) +std::string pose_delay_time_warning_message(const double delay_time) { const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string twistDelayTimeWarningMessage(const double delay_time) +std::string twist_delay_time_warning_message(const double delay_time) { const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string mahalanobisWarningMessage(const double distance, const double max_distance) +std::string mahalanobis_warning_message(const double distance, const double max_distance) { const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; return fmt::format(s, distance, max_distance); diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index 23458fb18a838..ed87ebdea029b 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -19,18 +19,18 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(0, 0) = 1.; - P(0, 1) = 2.; - P(0, 2) = 3.; - P(1, 0) = 4.; - P(1, 1) = 5.; - P(1, 2) = 6.; - P(2, 0) = 7.; - P(2, 1) = 8.; - P(2, 2) = 9.; + Matrix6d p = Matrix6d::Zero(); + p(0, 0) = 1.; + p(0, 1) = 2.; + p(0, 2) = 3.; + p(1, 0) = 4.; + p(1, 1) = 5.; + p(1, 2) = 6.; + p(2, 0) = 7.; + p(2, 1) = 8.; + p(2, 2) = 9.; - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[1], 2.); EXPECT_EQ(covariance[5], 3.); @@ -44,8 +44,8 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } @@ -55,13 +55,13 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(4, 4) = 1.; - P(4, 5) = 2.; - P(5, 4) = 3.; - P(5, 5) = 4.; + Matrix6d p = Matrix6d::Zero(); + p(4, 4) = 1.; + p(4, 5) = 2.; + p(5, 4) = 3.; + p(5, 5) = 4.; - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[5], 2.); EXPECT_EQ(covariance[30], 3.); @@ -70,8 +70,8 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp index f506dca1cb230..ef0d7a6493756 100644 --- a/localization/ekf_localizer/test/test_diagnostics.cpp +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -16,20 +16,20 @@ #include -TEST(TestEkfDiagnostics, CheckProcessActivated) +TEST(TestEkfDiagnostics, check_process_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; bool is_activated = true; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_activated = false; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, checkMeasurementUpdated) +TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,58 +38,58 @@ TEST(TestEkfDiagnostics, checkMeasurementUpdated) const size_t no_update_count_threshold_error = 250; size_t no_update_count = 0; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 1; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 49; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 50; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); no_update_count = 249; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); no_update_count = 250; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } -TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) +TEST(TestEkfDiagnostics, check_measurement_queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; const std::string measurement_type = "pose"; // not effect for stat.level size_t queue_size = 0; // not effect for stat.level - stat = checkMeasurementQueueSize(measurement_type, queue_size); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); queue_size = 1; // not effect for stat.level - stat = checkMeasurementQueueSize(measurement_type, queue_size); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); } -TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +TEST(TestEkfDiagnostics, check_measurement_delay_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -98,17 +98,17 @@ TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) const double delay_time_threshold = 1.0; // not effect for stat.level bool is_passed_delay_gate = true; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_delay_gate = false; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -117,19 +117,19 @@ TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level bool is_passed_mahalanobis_gate = true; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_mahalanobis_gate = false; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; std::vector stat_array(2); @@ -138,7 +138,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -146,7 +146,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -154,7 +154,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -162,7 +162,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -170,7 +170,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -178,7 +178,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -186,7 +186,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "ERROR0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); } diff --git a/localization/ekf_localizer/test/test_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp index d208c1e8da06b..db7d538b533c3 100644 --- a/localization/ekf_localizer/test/test_mahalanobis.cpp +++ b/localization/ekf_localizer/test/test_mahalanobis.cpp @@ -18,44 +18,44 @@ constexpr double tolerance = 1e-8; -TEST(SquaredMahalanobis, SmokeTest) +TEST(squared_mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance); } } -TEST(Mahalanobis, SmokeTest) +TEST(mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(mahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(mahalanobis(x, y, C), std::sqrt(5.0), tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance); } } diff --git a/localization/ekf_localizer/test/test_measurement.cpp b/localization/ekf_localizer/test/test_measurement.cpp index 9d63cb056d9d3..c77e39a67d15c 100644 --- a/localization/ekf_localizer/test/test_measurement.cpp +++ b/localization/ekf_localizer/test/test_measurement.cpp @@ -16,66 +16,66 @@ #include -TEST(Measurement, PoseMeasurementMatrix) +TEST(Measurement, pose_measurement_matrix) { - const Eigen::Matrix M = poseMeasurementMatrix(); + const Eigen::Matrix m = pose_measurement_matrix(); Eigen::Matrix expected; expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, TwistMeasurementMatrix) +TEST(Measurement, twist_measurement_matrix) { - const Eigen::Matrix M = twistMeasurementMatrix(); + const Eigen::Matrix m = twist_measurement_matrix(); Eigen::Matrix expected; expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, PoseMeasurementCovariance) +TEST(Measurement, pose_measurement_covariance) { { const std::array covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9}; - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2); Eigen::Matrix3d expected; expected << 2, 4, 6, 8, 10, 12, 14, 16, 18; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } -TEST(Measurement, TwistMeasurementCovariance) +TEST(Measurement, twist_measurement_covariance) { { const std::array covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4}; - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2); Eigen::Matrix2d expected; expected << 2, 4, 6, 8; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } diff --git a/localization/ekf_localizer/test/test_numeric.cpp b/localization/ekf_localizer/test/test_numeric.cpp index baf4f46db79a9..f84c9aa1d0bf2 100644 --- a/localization/ekf_localizer/test/test_numeric.cpp +++ b/localization/ekf_localizer/test/test_numeric.cpp @@ -18,30 +18,30 @@ #include -TEST(Numeric, HasNan) +TEST(Numeric, has_nan) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasNan(empty)); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 1., inf))); + EXPECT_FALSE(has_nan(empty)); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf))); - EXPECT_TRUE(hasNan(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.))); } -TEST(Numeric, HasInf) +TEST(Numeric, has_inf) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasInf(empty)); - EXPECT_FALSE(hasInf(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_FALSE(has_inf(empty)); + EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.))); - EXPECT_TRUE(hasInf(Eigen::Vector3d(0., 1., inf))); + EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf))); } diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp index 9cb7975a964a9..3b6bd93fd3dd8 100644 --- a/localization/ekf_localizer/test/test_state_transition.cpp +++ b/localization/ekf_localizer/test/test_state_transition.cpp @@ -17,43 +17,44 @@ #include "ekf_localizer/state_transition.hpp" #include -#include -TEST(StateTransition, NormalizeYaw) +#include + +TEST(StateTransition, normalize_yaw) { const double tolerance = 1e-6; - EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance); } -TEST(PredictNextState, PredictNextState) +TEST(predict_next_state, predict_next_state) { // This function is the definition of state transition so we just check // if the calculation is done according to the formula - Vector6d X_curr; - X_curr(0) = 2.; - X_curr(1) = 3.; - X_curr(2) = M_PI / 2.; - X_curr(3) = M_PI / 4.; - X_curr(4) = 10.; - X_curr(5) = 2. * M_PI / 3.; + Vector6d x_curr; + x_curr(0) = 2.; + x_curr(1) = 3.; + x_curr(2) = M_PI / 2.; + x_curr(3) = M_PI / 4.; + x_curr(4) = 10.; + x_curr(5) = 2. * M_PI / 3.; const double dt = 0.5; - const Vector6d X_next = predictNextState(X_curr, dt); + const Vector6d x_next = predict_next_state(x_curr, dt); const double tolerance = 1e-10; - EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance); - EXPECT_NEAR(X_next(3), X_curr(3), tolerance); - EXPECT_NEAR(X_next(4), X_curr(4), tolerance); - EXPECT_NEAR(X_next(5), X_curr(5), tolerance); + EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance); + EXPECT_NEAR(x_next(3), x_curr(3), tolerance); + EXPECT_NEAR(x_next(4), x_curr(4), tolerance); + EXPECT_NEAR(x_next(5), x_curr(5), tolerance); } -TEST(CreateStateTransitionMatrix, NumericalApproximation) +TEST(create_state_transition_matrix, NumericalApproximation) { // The transition matrix A = df / dx // We check if df = A * dx approximates f(x + dx) - f(x) @@ -64,10 +65,10 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = Vector6d::Zero(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 2e-3); + EXPECT_LT((df - a * dx).norm(), 2e-3); } { @@ -76,20 +77,20 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 5e-3); + EXPECT_LT((df - a * dx).norm(), 5e-3); } } -TEST(ProcessNoiseCovariance, ProcessNoiseCovariance) +TEST(process_noise_covariance, process_noise_covariance) { - const Matrix6d Q = processNoiseCovariance(1., 2., 3.); - EXPECT_EQ(Q(2, 2), 1.); // for yaw - EXPECT_EQ(Q(4, 4), 2.); // for vx - EXPECT_EQ(Q(5, 5), 3.); // for wz + const Matrix6d q = process_noise_covariance(1., 2., 3.); + EXPECT_EQ(q(2, 2), 1.); // for yaw + EXPECT_EQ(q(4, 4), 2.); // for vx + EXPECT_EQ(q(5, 5), 3.); // for wz // Make sure other elements are zero - EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.); + EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.); } diff --git a/localization/ekf_localizer/test/test_string.cpp b/localization/ekf_localizer/test/test_string.cpp index 7b35a56d889e9..013072e5145c7 100644 --- a/localization/ekf_localizer/test/test_string.cpp +++ b/localization/ekf_localizer/test/test_string.cpp @@ -16,11 +16,11 @@ #include -TEST(EraseLeadingSlash, SmokeTest) +TEST(erase_leading_slash, SmokeTest) { - EXPECT_EQ(eraseLeadingSlash("/topic"), "topic"); - EXPECT_EQ(eraseLeadingSlash("topic"), "topic"); // do nothing + EXPECT_EQ(erase_leading_slash("/topic"), "topic"); + EXPECT_EQ(erase_leading_slash("topic"), "topic"); // do nothing - EXPECT_EQ(eraseLeadingSlash(""), ""); - EXPECT_EQ(eraseLeadingSlash("/"), ""); + EXPECT_EQ(erase_leading_slash(""), ""); + EXPECT_EQ(erase_leading_slash("/"), ""); } diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index 2069969e0ae5e..ec7d3420d7f79 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -18,45 +18,45 @@ #include -TEST(PoseDelayStepWarningMessage, SmokeTest) +TEST(pose_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 4.0).c_str(), + pose_delay_step_warning_message(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " "delay: 6.000[s], limit: 4.000[s]"); } -TEST(TwistDelayStepWarningMessage, SmokeTest) +TEST(twist_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 6.0).c_str(), + twist_delay_step_warning_message(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " "delay: 10.000[s], limit: 6.000[s]"); } -TEST(PoseDelayTimeWarningMessage, SmokeTest) +TEST(pose_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayTimeWarningMessage(-1.0).c_str(), + pose_delay_time_warning_message(-1.0).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - poseDelayTimeWarningMessage(-0.4).c_str(), + pose_delay_time_warning_message(-0.4).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(TwistDelayTimeWarningMessage, SmokeTest) +TEST(twist_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayTimeWarningMessage(-1.0).c_str(), + twist_delay_time_warning_message(-1.0).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - twistDelayTimeWarningMessage(-0.4).c_str(), + twist_delay_time_warning_message(-0.4).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(MahalanobisWarningMessage, SmokeTest) +TEST(mahalanobis_warning_message, SmokeTest) { EXPECT_STREQ( - mahalanobisWarningMessage(1.0, 0.5).c_str(), + mahalanobis_warning_message(1.0, 0.5).c_str(), "The Mahalanobis distance 1.0000 is over the limit 0.5000."); }