From f7486dd3cd27af02ffb78f7224b18b67284ba90e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sat, 8 Jun 2024 15:03:05 +0200 Subject: [PATCH] Use std:: namespace for trigonometric functions --- .../src/steering_odometry.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index 9b97f51a74..9d40b00fc7 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -124,7 +124,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -150,7 +150,7 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_lin_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -317,8 +317,8 @@ void SteeringOdometry::integrate_runge_kutta_2( const double theta_mid = heading_ + omega_bz * 0.5 * dt; // Use the intermediate values to update the state - x_ += v_bx * cos(theta_mid) * dt; - y_ += v_bx * sin(theta_mid) * dt; + x_ += v_bx * std::cos(theta_mid) * dt; + y_ += v_bx * std::sin(theta_mid) * dt; heading_ += omega_bz * dt; } @@ -338,8 +338,8 @@ void SteeringOdometry::integrate_fk(const double v_bx, const double omega_bz, co const double heading_old = heading_; const double R = delta_x_b / delta_theta; heading_ += delta_theta; - x_ += R * (sin(heading_) - sin(heading_old)); - y_ += -R * (cos(heading_) - cos(heading_old)); + x_ += R * (sin(heading_) - std::sin(heading_old)); + y_ += -R * (cos(heading_) - std::cos(heading_old)); } }