diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index 750aad2407..f56e05dbb1 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -130,11 +130,11 @@ class SteeringOdometry /** * \brief Updates the odometry class with latest velocity command - * \param linear Linear velocity [m/s] - * \param angular Angular velocity [rad/s] + * \param v_bx Linear velocity [m/s] + * \param omega_bz Angular velocity [rad/s] * \param dt time difference to last call */ - void update_open_loop(const double linear, const double angular, const double dt); + void update_open_loop(const double v_bx, const double omega_bz, const double dt); /** * \brief Set odometry type @@ -186,8 +186,8 @@ class SteeringOdometry /** * \brief Calculates inverse kinematics for the desired linear and angular velocities - * \param v_bx Linear velocity of the robot in x_b-axis direction - * \param omega_bz Angular velocity of the robot around x_z-axis + * \param v_bx Desired linear velocity of the robot in x_b-axis direction + * \param omega_bz Desired angular velocity of the robot around x_z-axis * \return Tuple of velocity commands and steering commands */ std::tuple, std::vector> get_commands( diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index de1182132f..45323c3f82 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -174,14 +174,14 @@ bool SteeringOdometry::update_from_velocity( return update_odometry(linear_velocity, angular_velocity, dt); } -void SteeringOdometry::update_open_loop(const double linear, const double angular, const double dt) +void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz, const double dt) { /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; + linear_ = v_bx; + angular_ = omega_bz; /// Integrate odometry: - integrate_fk(linear, angular, dt); + integrate_fk(v_bx, omega_bz, dt); } void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track)