bool Odometry::update(double wheel1_vel, double wheel2_vel, double wheel3_vel, const ros::Time &time)
{
/// Get current wheel joint positions:
//because of the matrix the value's squence is a little different than us
const double v1 = wheel2_vel * wheel_radius_;
const double v2 = wheel3_vel * wheel_radius_;
const double v3 = wheel1_vel * wheel_radius_;
const double L = wheel_center_;
const double th = heading_;
const double psi = 2 * 3.1415926 / 3;
/// Update old position with current:
wheel1_pos_ += v1;
wheel2_pos_ += v2;
wheel3_pos_ += v3;
/// Compute linear and angular omni:
// const double linear_x = (v1*(cos(psi/2 + th) + cos(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) - (v3*(cos(psi/2 + th) - cos(psi/2 - th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) - (v2*(cos(psi/2 - th) + cos(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th));
// const double linear_y = (v2*(sin(psi/2 - th) - sin(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) - (v3*(sin(psi/2 + th) + sin(psi/2 - th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)) + (v1*(sin(psi/2 + th) + sin(th)))/(sin(psi/2 + th)*cos(th) - cos(psi/2 + th)*sin(th) + cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th) + cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th));
// const double angular = (v3*(cos(psi/2 + th)*sin(psi/2 - th) + sin(psi/2 + th)*cos(psi/2 - th)))/(L*cos(psi/2 + th)*sin(psi/2 - th) + L*sin(psi/2 + th)*cos(psi/2 - th) - L*cos(psi/2 + th)*sin(th) + L*sin(psi/2 + th)*cos(th) + L*cos(psi/2 - th)*sin(th) + L*sin(psi/2 - th)*cos(th)) - (v1*(cos(psi/2 + th)*sin(th) - sin(psi/2 + th)*cos(th)))/(L*cos(psi/2 + th)*sin(psi/2 - th) + L*sin(psi/2 + th)*cos(psi/2 - th) - L*cos(psi/2 + th)*sin(th) + L*sin(psi/2 + th)*cos(th) + L*cos(psi/2 - th)*sin(th) + L*sin(psi/2 - th)*cos(th)) + (v2*(cos(psi/2 - th)*sin(th) + sin(psi/2 - th)*cos(th)))/(L*cos(psi/2 + th)*sin(psi/2 - th) + L*sin(psi/2 + th)*cos(psi/2 - th) - L*cos(psi/2 + th)*sin(th) + L*sin(psi/2 + th)*cos(th) + L*cos(psi/2 - th)*sin(th) + L*sin(psi/2 - th)*cos(th));
const double linear_x = (v1 / 2 / sin(psi / 2) - v2 / 2 / sin(psi / 2));
const double linear_y = -v3 * 2 / 3 + v1 / 3 + v2 / 3;
const double angular = v1 / 2 / (L + L / cos(psi / 2)) / cos(psi / 2) + v2 / 2 / (L + L / cos(psi / 2)) / cos(psi / 2) + v3 / (L + L / cos(psi / 2));
ROS_INFO("velocity is %lf %lf %lf", wheel1_vel, wheel2_vel, wheel3_vel);
/// We cannot estimate the speed with very small time intervals:
const double dt = (time - timestamp_).toSec();
if(dt < 0.0001)
return false; // Interval too small to integrate with
timestamp_ = time;
/// Integrate odometry:
integrate_fun_(linear_x, -linear_y, angular);
/// Estimate speeds using a rolling mean to filter them out:
// attention!!!!!! becaunse of the wrong direction of the axis
linear_x_acc_(linear_x / dt);
linear_y_acc_(linear_y / dt);
angular_acc_(angular / dt);
linear_x_ = bacc::rolling_mean(linear_x_acc_);
linear_y_ = bacc::rolling_mean(linear_y_acc_);
angular_ = bacc::rolling_mean(angular_acc_);
return true;
}
bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
{
/// Get current wheel joint positions:
const double left_wheel_cur_pos = left_pos * wheel_radius_;
const double right_wheel_cur_pos = right_pos * wheel_radius_;
/// 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_;
/// Update old position with current:
left_wheel_old_pos_ = left_wheel_cur_pos;
right_wheel_old_pos_ = right_wheel_cur_pos;
/// Compute linear and angular diff:
const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
const double angular = _slipFactor*((right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_);
/// Integrate odometry:
integrate_fun_(linear, (angular));
/// We cannot estimate the speed with very small time intervals:
const double dt = (time - timestamp_).toSec();
if (dt < 0.0001)
return false; // Interval too small to integrate with
timestamp_ = time;
/// Estimate speeds using a rolling mean to filter them out:
linear_acc_(linear/dt);
angular_acc_(angular/dt);
linear_ = bacc::rolling_mean(linear_acc_);
angular_ = bacc::rolling_mean(angular_acc_);
return true;
}