clang

CCNYRoboticsLab · May 30, 2024 · 7c0e0d9 · 7c0e0d9
1 parent cb1747c
commit 7c0e0d9
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 17 deletions.
diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h
@@ -103,7 +103,6 @@ class ImuFilterRos
     ros::Duration time_jump_threshold_;
     ros::Duration forward_large_time_jump_threshold_;
 
-
     // **** state variables
     boost::mutex mutex_;
     bool initialized_;
@@ -134,4 +133,4 @@ class ImuFilterRos
     bool checkLargeTimeJumpForward(const ros::Time& imu_stamp);
 };
 
-#endif  // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H
+#endif  // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H
diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp
@@ -54,14 +54,19 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private)
                       time_jump_threshold);
     time_jump_threshold_ = ros::Duration(time_jump_threshold);
 
-    if (!nh_private_.getParam("refuse_large_time_update", refuse_large_time_update_)) refuse_large_time_update_ = true;
-    if (!nh_private_.getParam("reset_large_forward_time_jump", reset_large_forward_time_jump_)) reset_large_forward_time_jump_ = false;
-
+    if (!nh_private_.getParam("refuse_large_time_update",
+                              refuse_large_time_update_))
+        refuse_large_time_update_ = true;
+    if (!nh_private_.getParam("reset_large_forward_time_jump",
+                              reset_large_forward_time_jump_))
+        reset_large_forward_time_jump_ = false;
 
     double forward_large_time_jump_threshold{1.0};
-    nh_private_.param("forward_large_time_jump_threshold", forward_large_time_jump_threshold,
+    nh_private_.param("forward_large_time_jump_threshold",
+                      forward_large_time_jump_threshold,
                       forward_large_time_jump_threshold);
-    forward_large_time_jump_threshold_ = ros::Duration(forward_large_time_jump_threshold);
+    forward_large_time_jump_threshold_ =
+        ros::Duration(forward_large_time_jump_threshold);
 
     double yaw_offset = 0.0;
     if (!nh_private_.getParam("yaw_offset", yaw_offset)) yaw_offset = 0.0;
@@ -179,7 +184,7 @@ void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
     checkTimeJump();
     if (checkLargeTimeJumpForward(imu_msg_raw->header.stamp))
     {
-        return; // large jump don't use reading
+        return;  // large jump don't use reading
     }
 
     boost::mutex::scoped_lock lock(mutex_);
@@ -245,10 +250,9 @@ void ImuFilterRos::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
     checkTimeJump();
     if (checkLargeTimeJumpForward(imu_msg_raw->header.stamp))
     {
-        return; // large jump don't use reading
+        return;  // large jump don't use reading
     }
 
-
     boost::mutex::scoped_lock lock(mutex_);
 
     const geometry_msgs::Vector3& ang_vel = imu_msg_raw->angular_velocity;
@@ -541,22 +545,22 @@ bool ImuFilterRos::checkLargeTimeJumpForward(const ros::Time& imu_stamp)
     bool largeJump = false;
     if (constant_dt_ > 0.0)
     {
-        return largeJump; // use constant dt
+        return largeJump;  // use constant dt
     }
 
-    if (imu_stamp > last_time_ + forward_large_time_jump_threshold_
-    && !last_time_.isZero())
+    if (imu_stamp > last_time_ + forward_large_time_jump_threshold_ &&
+        !last_time_.isZero())
     {
-        ROS_ERROR("Detected large jump forward, jump is %f[s]. rejecting measurement",
-        (imu_stamp - last_time_).toSec());
+        ROS_ERROR(
+            "Detected large jump forward, jump is %f[s]. rejecting measurement",
+            (imu_stamp - last_time_).toSec());
         largeJump = true;
-        last_time_ = imu_stamp; // give time for next move
+        last_time_ = imu_stamp;  // give time for next move
         if (reset_large_forward_time_jump_)
         {
             reset();
             largeJump = false;
         }
-
     }
 
     return largeJump;