Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

drop or reset upon large forward time jump #200

Open
wants to merge 5 commits into
base: noetic
Choose a base branch
from
Open

drop or reset upon large forward time jump #200

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8c0c696
drop or reset upon large forward time jump
nachumlerner May 30, 2024
cb1747c
trim white space
nachumlerner May 30, 2024
7c0e0d9
clang
nachumlerner May 30, 2024
d340f03
pre commit hook
nachumlerner May 31, 2024
4b81f93
pre compile hook
nachumlerner May 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,8 @@ class ImuFilterRos
bool stateless_;
bool publish_tf_;
bool reverse_tf_;
bool refuse_large_time_update_;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This variable is never read and can be removed.

bool reset_large_forward_time_jump_;
std::string fixed_frame_;
std::string imu_frame_;
double constant_dt_;
Expand All @@ -99,6 +101,7 @@ class ImuFilterRos
geometry_msgs::Vector3 mag_bias_;
double orientation_variance_;
ros::Duration time_jump_threshold_;
ros::Duration forward_large_time_jump_threshold_;

// **** state variables
boost::mutex mutex_;
Expand Down Expand Up @@ -127,6 +130,7 @@ class ImuFilterRos
void applyYawOffset(double& q0, double& q1, double& q2, double& q3);
//! \brief Check whether ROS time has jumped back. If so, reset the filter.
void checkTimeJump();
bool checkLargeTimeJumpForward(const ros::Time& imu_stamp);
};

#endif // IMU_FILTER_IMU_MADWICK_FILTER_ROS_H
48 changes: 48 additions & 0 deletions imu_filter_madgwick/src/imu_filter_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,20 @@ 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;

double forward_large_time_jump_threshold{1.0};
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);

double yaw_offset = 0.0;
if (!nh_private_.getParam("yaw_offset", yaw_offset)) yaw_offset = 0.0;
double declination = 0.0;
Expand Down Expand Up @@ -168,6 +182,10 @@ ImuFilterRos::~ImuFilterRos()
void ImuFilterRos::imuCallback(const ImuMsg::ConstPtr& imu_msg_raw)
{
checkTimeJump();
if (checkLargeTimeJumpForward(imu_msg_raw->header.stamp))
{
return; // large jump don't use reading
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't understand this comment. It should be something like "reject measurement if large time jump forward detected".

(repeated on line 253)

}

boost::mutex::scoped_lock lock(mutex_);

Expand Down Expand Up @@ -230,6 +248,10 @@ void ImuFilterRos::imuMagCallback(const ImuMsg::ConstPtr& imu_msg_raw,
const MagMsg::ConstPtr& mag_msg)
{
checkTimeJump();
if (checkLargeTimeJumpForward(imu_msg_raw->header.stamp))
{
return; // large jump don't use reading
}

boost::mutex::scoped_lock lock(mutex_);

Expand Down Expand Up @@ -517,3 +539,29 @@ void ImuFilterRos::checkTimeJump()

reset();
}

bool ImuFilterRos::checkLargeTimeJumpForward(const ros::Time& imu_stamp)
{
bool largeJump = false;
if (constant_dt_ > 0.0)
{
return largeJump; // use constant dt
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This can be simplified to return false;.

Also, the comment is not very helpful. It should be something like "don't reject measurement on large jump if using constant dt". But why does that make sense? Shouldn't we do the opposite (always reject if using constant_dt)?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We will change the comment and the return statement. If dt is constant, it doesn't matter what the jump in the timestamp is, so we should use the measurement.

}

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());
largeJump = true;
last_time_ = imu_stamp; // save time for next move
if (reset_large_forward_time_jump_)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I wonder if we shouldn't always reset. This would simplify the code a lot:

  • we can remove the reset_large_forward_time_jump parameter
  • we can probably even remove the return value from this function, and the largeJump variable
  • we can make this function very similar to checkTimeJump(), and perhaps even merge this function into it.

Also, it sounds safer to always reset. The current implementation only rejects the first measurement after a large jump forward, but if the filter isn't reset, the next message will still have a large jump in values with a small dt, so rejecting the first measurement doesn't help at all.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think we should always reset. In our application, we experience one large time jump when our robot establishes communication. It could be very dangerous to reset the IMU suddenly. Instead, I believe it is better to ignore one measurement. Why should the next message have a large jump in values?

Copy link
Collaborator

@mintar mintar May 31, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah yes, after reading #160 again I remember the dilemma in your application. There can be two sources of large forward time jumps between two consecutive messages:

  1. Pause: If the connection to the IMU was lost somehow and messages were dropped for an extensive time, when the first message arrives again you get a large time jump and a large jump in values (because the IMU was moved in the meantime).
  2. Clock update: There is no connection error, and the IMU messages arrive continuously at the same rate, but the clock was moved forward between two messages. There's a large time jump but no large jump in values.

You're trying to deal with case 2 here, but I was thinking about case 1.

As I already wrote in #160 (comment), I think it's a bad idea to try to handle case 2 in the imu_filter_madgwick.

The source of the problem is the incorrect system clock. You are feeding bad data with incorrect time stamps (and a time jump) to all downstream software (such as imu_filter_madgwick). This will cause bad behavior in hundreds of different places, often with very subtle effects that will be a nightmare to track down.

Instead of trying to make the downstream software deal with incorrect inputs, you should fix the problem at the source and provide correct time stamps. Two wrongs don't make a right.

There are many ways you could fix the time jumps:

  1. Add a real time clock to the robot (they only cost a couple of bucks).
  2. Don't update the time after connecting to the internet.
  3. First connect to the internet, wait until the time is updated, then start the ROS software.
  4. Detect when time jumps happen and restart the ROS software then.
  5. ...

{
reset();
largeJump = false;
}
}

return largeJump;
}