Skip to content

Commit

Permalink
Rename method and variables
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed May 24, 2024
1 parent a20ff13 commit 2d76c44
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -221,7 +221,7 @@ class SteeringOdometry
* \param omega_bz Angular velocity [rad/s]
* \param dt time difference to last call
*/
void integrate(const double v_bx, const double omega_bz, const double dt);
void integrate_fk(const double v_bx, const double omega_bz, const double dt);

/**
* \brief Calculates steering angle from the desired translational and rotational velocity
Expand Down
13 changes: 7 additions & 6 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,11 @@ void SteeringOdometry::init(const rclcpp::Time & time)
timestamp_ = time;
}

bool SteeringOdometry::update_odometry(const double linear, const double angular, const double dt)
bool SteeringOdometry::update_odometry(
const double linear_velocity, const double angular_velocity, const double dt)
{
/// Integrate odometry:
integrate(linear, angular, dt);
integrate_fk(linear_velocity, angular_velocity, dt);

/// We cannot estimate the speed with very small time intervals:
if (dt < 0.0001)
Expand All @@ -60,8 +61,8 @@ bool SteeringOdometry::update_odometry(const double linear, const double angular
}

/// Estimate speeds using a rolling mean to filter them out:
linear_acc_.accumulate(linear);
angular_acc_.accumulate(angular);
linear_acc_.accumulate(linear_velocity);
angular_acc_.accumulate(angular_velocity);

linear_ = linear_acc_.getRollingMean();
angular_ = angular_acc_.getRollingMean();
Expand Down Expand Up @@ -180,7 +181,7 @@ void SteeringOdometry::update_open_loop(const double linear, const double angula
angular_ = angular;

/// Integrate odometry:
integrate(linear, angular, dt);
integrate_fk(linear, angular, dt);
}

void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)
Expand Down Expand Up @@ -302,7 +303,7 @@ void SteeringOdometry::integrate_runge_kutta_2(
heading_ += omega_bz * dt;
}

void SteeringOdometry::integrate(const double v_bx, const double omega_bz, const double dt)
void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, const double dt)
{
const double delta_x_b = v_bx * dt;
const double delta_Theta = omega_bz * dt;
Expand Down

0 comments on commit 2d76c44

Please sign in to comment.