Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Jun 6, 2024
1 parent 52e7511 commit aa614ae
Show file tree
Hide file tree
Showing 2 changed files with 9 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,8 @@
#include <nav_msgs/msg/odometry.hpp>

#include <deque>
#include <vector>
#include <tuple>
#include <vector>

class PoseInstabilityDetector : public rclcpp::Node
{
Expand All @@ -39,15 +39,16 @@ class PoseInstabilityDetector : public rclcpp::Node
using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;

public:
struct ThresholdValues {
struct ThresholdValues
{
double position_x;
double position_y;
double position_z;
double angle_x;
double angle_y;
double angle_z;
};

explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
ThresholdValues calculate_threshold(double interval_sec);
void dead_reckon(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,8 @@ void PoseInstabilityDetector::callback_timer()
diff_pose_pub_->publish(diff_pose);

// publish diagnostics
ThresholdValues threshold_values = calculate_threshold((latest_odometry_time - prev_odometry_time).seconds());
ThresholdValues threshold_values =
calculate_threshold((latest_odometry_time - prev_odometry_time).seconds());

const std::vector<double> thresholds = {threshold_values.position_x, threshold_values.position_y,
threshold_values.position_z, threshold_values.angle_x,
Expand Down Expand Up @@ -176,7 +177,8 @@ void PoseInstabilityDetector::callback_timer()
prev_odometry_ = latest_odometry_;
}

Check notice on line 178 in localization/pose_instability_detector/src/pose_instability_detector.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

PoseInstabilityDetector::callback_timer decreases in cyclomatic complexity from 11 to 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold(double interval_sec)
PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold(
double interval_sec)
{
// Calculate maximum longitudinal difference
const double longitudinal_difference =
Expand Down Expand Up @@ -323,7 +325,7 @@ PoseInstabilityDetector::clip_out_necessary_twist(
// get iterator to the element that is right before start_time (if it does not exist, start_it =
// twist_buffer.begin())
auto start_it = twist_buffer.begin();

for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) {
if (rclcpp::Time(it->header.stamp) > start_time) {
break;
Expand Down

0 comments on commit aa614ae

Please sign in to comment.