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

fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

Merged
merged 17 commits into from
Aug 7, 2024
Merged

fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch #8095

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
f3f773c
fix(ekf_localizer)add_delay_compensation_for_roll_and_pitch
meliketanrikulu Jul 18, 2024
91f6146
rename compensate_pose_with_delay function
meliketanrikulu Jul 23, 2024
bbb9561
rename variables
meliketanrikulu Jul 23, 2024
8a16c03
update comment
meliketanrikulu Jul 23, 2024
b826298
rename compensated pose name
meliketanrikulu Jul 23, 2024
b7327c7
rename function name
meliketanrikulu Jul 23, 2024
8316155
style(pre-commit): autofix
pre-commit-ci[bot] Jul 23, 2024
4993564
calculate delta_orientation with orientation
meliketanrikulu Jul 29, 2024
03309e2
rename - angular velocity vector
meliketanrikulu Jul 29, 2024
1ddbee8
add z element delay compensation
meliketanrikulu Jul 30, 2024
3398927
fix typo
meliketanrikulu Jul 30, 2024
a061f42
check twist msg for last_angular_velocity_
meliketanrikulu Jul 30, 2024
5af1316
use corrected pitch for delta_z calculation
meliketanrikulu Jul 30, 2024
622c076
update pose_with_delay header stamp
meliketanrikulu Jul 30, 2024
1acb118
style(pre-commit): autofix
pre-commit-ci[bot] Jul 30, 2024
e33ab61
fix local variable naming
meliketanrikulu Jul 31, 2024
396753d
Merge branch 'main' into fix(ekf_localizer)add_delay_compensation_for…
YamatoAndo Aug 2, 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: 3 additions & 1 deletion localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class EKFModule
bool measurement_update_twist(
const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
EKFDiagnosticInfo & twist_diag_info);
geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay(
geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay(
const PoseWithCovariance & pose, const double delay_time);

private:
Expand All @@ -86,6 +86,8 @@ class EKFModule
std::shared_ptr<Warning> warning_;
const int dim_x_;
std::vector<double> accumulated_delay_times_;
double roll_rate_;
double pitch_rate_;
const HyperParameters params_;
};

Expand Down
8 changes: 5 additions & 3 deletions localization/ekf_localizer/src/ekf_localizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -187,11 +187,13 @@
if (is_updated) {
pose_is_updated = true;

// Update Simple 1D filter with considering change of z value due to measurement pose delay
// Update Simple 1D filter with considering change of roll, pitch and height (position z)
// values due to measurement pose delay
const double delay_time =
(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);
auto pose_with_rph_delay_compensation =
ekf_module_->compensate_rph_with_delay(*pose, delay_time);
update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps);

Check warning on line 196 in localization/ekf_localizer/src/ekf_localizer.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

EKFLocalizer::timer_callback already has high cyclomatic complexity, and now it increases in Lines of Code from 79 to 80. 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.
}
}
DEBUG_INFO(
Expand Down
26 changes: 20 additions & 6 deletions localization/ekf_localizer/src/ekf_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
: warning_(std::move(warning)),
dim_x_(6), // x, y, yaw, yaw_bias, vx, wz
accumulated_delay_times_(params.extend_state_step, 1.0E15),
roll_rate_(0.0),
pitch_rate_(0.0),
params_(params)
{
Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1);
Expand Down Expand Up @@ -282,15 +284,25 @@
return true;
}

geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay(
geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay(

Check warning on line 287 in localization/ekf_localizer/src/ekf_module.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ekf_localizer/src/ekf_module.cpp#L287 

Added line #L287 was not covered by tests
const PoseWithCovariance & pose, const double delay_time)
{
const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation);
const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
PoseWithCovariance pose_with_z_delay;
pose_with_z_delay = pose;
pose_with_z_delay.pose.pose.position.z += dz_delay;
return pose_with_z_delay;
const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
TaikiYamada4 marked this conversation as resolved.
Show resolved Hide resolved
const double delta_roll = roll_rate_ * delay_time;
const double delta_pitch = pitch_rate_ * delay_time;

Check warning on line 293 in localization/ekf_localizer/src/ekf_module.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ekf_localizer/src/ekf_module.cpp#L291-L293 

Added lines #L291 - L293 were not covered by tests

PoseWithCovariance pose_with_delay;
pose_with_delay = pose;
pose_with_delay.pose.pose.position.z += delta_z;
const double corrected_roll = rpy.x + delta_roll;
const double corrected_pitch = rpy.y + delta_pitch;

Check warning on line 299 in localization/ekf_localizer/src/ekf_module.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ekf_localizer/src/ekf_module.cpp#L297-L299 

Added lines #L297 - L299 were not covered by tests
tf2::Quaternion quat;
quat.setRPY(corrected_roll, corrected_pitch, rpy.z);
quat.normalize();
pose_with_delay.pose.pose.orientation = tf2::toMsg(quat);

Check warning on line 303 in localization/ekf_localizer/src/ekf_module.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ekf_localizer/src/ekf_module.cpp#L301-L303 

Added lines #L301 - L303 were not covered by tests

return pose_with_delay;

Check warning on line 305 in localization/ekf_localizer/src/ekf_module.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ekf_localizer/src/ekf_module.cpp#L305 

Added line #L305 was not covered by tests
}

bool EKFModule::measurement_update_twist(
Expand Down Expand Up @@ -329,6 +341,8 @@
/* Set measurement matrix */
Eigen::MatrixXd y(dim_y, 1);
y << twist.twist.twist.linear.x, twist.twist.twist.angular.z;
roll_rate_ = twist.twist.twist.angular.x;
pitch_rate_ = twist.twist.twist.angular.y;

Check warning on line 345 in localization/ekf_localizer/src/ekf_module.cpp

View check run for this annotation

Codecov / codecov/patch

localization/ekf_localizer/src/ekf_module.cpp#L344-L345 

Added lines #L344 - L345 were not covered by tests
Copy link

Choose a reason for hiding this comment

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

Suggestion: To avoid potential issues with uninitialized variables, ensure that roll_rate_ and pitch_rate_ are initialized directly with the values from twist. [possible issue, importance: 7]

Suggested change
roll_rate_ = twist.twist.twist.angular.x;
pitch_rate_ = twist.twist.twist.angular.y;
roll_rate_ = twist.twist.twist.angular.x != 0.0 ? twist.twist.twist.angular.x : roll_rate_;
pitch_rate_ = twist.twist.twist.angular.y != 0.0 ? twist.twist.twist.angular.y : pitch_rate_;

Copy link
Contributor

Choose a reason for hiding this comment

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

I believe that taking the independent factor of the angular velocity vector as roll and pitch velocity is not mathematically correct, because the norm of the angular velocity vector equals to the angular variation around the major axis. In other words, the roll_rate_ and pitch_rate_ can be affected by other factors.

In my opinion, calculating the variation using the quaternion library should be better for precise calculation of angular variation. I think Line 270 to 281 in pose_instability_detector.cpp might be helpful.

Yes, this also calculates the variation of yaw, but since yaw is estimated in a different way we can discard the yaw part after the calculation of angular variation.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Hello @TaikiYamada4 . Thanks for your review. You are right, it is a more accurate approach to calculate orientation as a whole instead of roll_rate and pitch_rate. I updated the code accordingly and tested it. 4993564
Could you check again?

Copy link
Contributor

Choose a reason for hiding this comment

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

@meliketanrikulu Thank you for the revision! I also checked it seems to work in my environment too 👍


if (has_nan(y) || has_inf(y)) {
warning_->warn(
Expand Down
Loading