Skip to content

Commit

Permalink
fix(motion_velocity_smoother): front wheel steer rate calculation (#5965
Browse files Browse the repository at this point in the history
)

#5964

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
  • Loading branch information
VRichardJP authored Dec 28, 2023
1 parent 6efa91b commit e4e129e
Showing 1 changed file with 2 additions and 2 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -224,8 +224,8 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit(
auto & steer_back = output.at(i).front_wheel_angle_rad;

// calculate the just 2 steering angle
steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i));
steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i + 1));
steer_front = std::atan(base_param_.wheel_base * curvature_v.at(i + 1));
steer_back = std::atan(base_param_.wheel_base * curvature_v.at(i));

const auto mean_vel = 0.5 * (v_front + v_back);
const auto dt = std::max(points_interval / mean_vel, std::numeric_limits<double>::epsilon());
Expand Down

0 comments on commit e4e129e

Please sign in to comment.