Skip to content

Commit

Permalink
fix(steering-odometry): convert twist to steering angle (#1288)
Browse files Browse the repository at this point in the history
(cherry picked from commit 50036e1)
  • Loading branch information
reinzor authored and mergify[bot] committed Sep 25, 2024
1 parent 448cf43 commit 90dba26
Showing 1 changed file with 3 additions and 6 deletions.
9 changes: 3 additions & 6 deletions steering_controllers_library/src/steering_odometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -209,12 +209,9 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_

double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz)
{
if (fabs(v_bx) < std::numeric_limits<float>::epsilon())
{
// avoid division by zero
return 0.;
}
return std::atan(omega_bz * wheelbase_ / v_bx);
// phi can be nan if both v_bx and omega_bz are zero
const auto phi = std::atan(omega_bz * wheelbase_ / v_bx);
return std::isfinite(phi) ? phi : 0.0;
}

std::tuple<std::vector<double>, std::vector<double>> SteeringOdometry::get_commands(
Expand Down

0 comments on commit 90dba26

Please sign in to comment.