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

update old pos before publishing odom #599

Open
wants to merge 3 commits into
base: noetic-devel
Choose a base branch
from
Open

update old pos before publishing odom #599

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ce2548f
validate old joint position data in order to calculate true velocity.
1hada May 18, 2022
577e180
update comment to briefly describe need for initially valid wheel pos…
1hada May 18, 2022
5c6ed99
Merge branch 'ros-controls:noetic-devel' into noetic-devel
1hada May 19, 2022
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
1 change: 1 addition & 0 deletions diff_drive_controller/include/diff_drive_controller/odometry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,7 @@ namespace diff_drive_controller
/// Previou wheel position/state [rad]:
double left_wheel_old_pos_;
double right_wheel_old_pos_;
bool old_pos_valid_;

/// Rolling mean accumulators for the linar and angular velocities:
size_t velocity_rolling_window_size_;
Expand Down
11 changes: 11 additions & 0 deletions diff_drive_controller/src/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ namespace diff_drive_controller
, right_wheel_radius_(0.0)
, left_wheel_old_pos_(0.0)
, right_wheel_old_pos_(0.0)
, old_pos_valid_(false)
, velocity_rolling_window_size_(velocity_rolling_window_size)
, linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
Expand All @@ -79,6 +80,15 @@ namespace diff_drive_controller
const double left_wheel_cur_pos = left_pos * left_wheel_radius_;
const double right_wheel_cur_pos = right_pos * right_wheel_radius_;

if (!old_pos_valid_)
{
/// Set old position with current to retain true velocities when initial wheel position is non-zero:
left_wheel_old_pos_ = left_wheel_cur_pos;
right_wheel_old_pos_ = right_wheel_cur_pos;
old_pos_valid_ = true;
return false;
}

/// Estimate velocity of wheels using old and current position:
const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
Expand Down Expand Up @@ -171,6 +181,7 @@ namespace diff_drive_controller
{
linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
old_pos_valid_ = false;
}

} // namespace diff_drive_controller