diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 0f87e78f84..99ad3950de 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -46,11 +46,11 @@ void GoalAngleCritic::score(CriticData & data) if (power_ > 1u) { data.costs += xt::pow( - xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * + xt::mean(xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_, power_); } else { data.costs += xt::mean( - xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_; + xt::fabs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) * weight_; } } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 491adc4580..c0bc625841 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -106,7 +106,7 @@ void PathAngleCritic::score(CriticData & data) case PathAngleMode::FORWARD_PREFERENCE: { auto yaws = - xt::abs(utils::shortest_angular_distance( + xt::fabs(utils::shortest_angular_distance( xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points)); if (power_ > 1u) { data.costs += xt::pow(std::move(yaws) * weight_, power_); @@ -118,11 +118,11 @@ void PathAngleCritic::score(CriticData & data) case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { auto yaws = - xt::abs(utils::shortest_angular_distance( + xt::fabs(utils::shortest_angular_distance( xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points)); const auto yaws_between_points_corrected = xt::where( yaws < M_PIF_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF)); - const auto corrected_yaws = xt::abs( + const auto corrected_yaws = xt::fabs( utils::shortest_angular_distance( xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected)); if (power_ > 1u) { @@ -135,9 +135,9 @@ void PathAngleCritic::score(CriticData & data) case PathAngleMode::CONSIDER_FEASIBLE_PATH_ORIENTATIONS: { const auto yaws_between_points_corrected = xt::where( - xt::abs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PIF_2, + xt::fabs(utils::shortest_angular_distance(yaws_between_points, goal_yaw)) < M_PIF_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF)); - const auto corrected_yaws = xt::abs( + const auto corrected_yaws = xt::fabs( utils::shortest_angular_distance( xt::view(data.trajectories.yaws, xt::all(), -1), yaws_between_points_corrected)); if (power_ > 1u) { diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index a407ad89ce..f4ae970dc6 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -38,9 +38,9 @@ void TwirlingCritic::score(CriticData & data) } if (power_ > 1u) { - data.costs += xt::pow(xt::mean(xt::abs(data.state.wz), {1}, immediate) * weight_, power_); + data.costs += xt::pow(xt::mean(xt::fabs(data.state.wz), {1}, immediate) * weight_, power_); } else { - data.costs += xt::mean(xt::abs(data.state.wz), {1}, immediate) * weight_; + data.costs += xt::mean(xt::fabs(data.state.wz), {1}, immediate) * weight_; } }