From 7a0e651fc35999efe88de9b9d853bb9b858472f2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 8 Mar 2024 15:54:52 -0800 Subject: [PATCH] 18.5% performance improvement in MPPI --- .../nav2_mppi_controller/critic_manager.hpp | 3 +- .../critics/cost_critic.hpp | 9 +-- .../nav2_mppi_controller/tools/utils.hpp | 30 +++++----- nav2_mppi_controller/src/critic_manager.cpp | 4 +- .../src/critics/constraint_critic.cpp | 50 ++++++++++------ .../src/critics/cost_critic.cpp | 59 +++++++++---------- .../src/critics/goal_angle_critic.cpp | 4 +- .../src/critics/goal_critic.cpp | 12 ++-- .../src/critics/obstacles_critic.cpp | 10 ++-- .../src/critics/path_align_critic.cpp | 14 ++--- .../src/critics/path_angle_critic.cpp | 16 ++--- .../src/critics/path_follow_critic.cpp | 10 +--- .../src/critics/prefer_forward_critic.cpp | 7 +-- .../src/critics/twirling_critic.cpp | 5 +- 14 files changed, 116 insertions(+), 117 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index 2ae8a38513..2de3e72a29 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -42,6 +42,7 @@ namespace mppi class CriticManager { public: + typedef std::vector> Critics; /** * @brief Constructor for mppi::CriticManager */ @@ -94,7 +95,7 @@ class CriticManager ParametersHandler * parameters_handler_; std::vector critic_names_; std::unique_ptr> loader_; - std::vector> critics_; + Critics critics_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index 14c0fe93f4..fdbe91286d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -58,14 +58,6 @@ class CostCritic : public CriticFunction */ bool inCollision(float cost, float x, float y, float theta); - /** - * @brief cost at a robot pose - * @param x X of pose - * @param y Y of pose - * @return Collision information at pose - */ - float costAtPose(float x, float y); - /** * @brief Find the min cost of the inflation decay function for which the robot MAY be * in collision in any orientation @@ -81,6 +73,7 @@ class CostCritic : public CriticFunction float possible_collision_cost_; bool consider_footprint_{true}; + bool is_tracking_unknown_{true}; float circumscribed_radius_{0}; float circumscribed_cost_{0}; float collision_cost_{0}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 49ff2cb010..763ea64642 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -49,6 +49,9 @@ #include "builtin_interfaces/msg/time.hpp" #include "nav2_mppi_controller/critic_data.hpp" +#define M_PIF 3.141592653589793238462643383279502884e+00F +#define M_PIF_2 1.5707963267948966e+00F + namespace mppi::utils { using xt::evaluation_strategy::immediate; @@ -259,7 +262,7 @@ inline bool withinPositionGoalTolerance( /** * @brief normalize - * Normalizes the angle to be -M_PI circle to +M_PI circle + * Normalizes the angle to be -M_PIF circle to +M_PIF circle * It takes and returns radians. * @param angles Angles to normalize * @return normalized angles @@ -267,8 +270,8 @@ inline bool withinPositionGoalTolerance( template auto normalize_angles(const T & angles) { - auto && theta = xt::eval(xt::fmod(angles + M_PI, 2.0 * M_PI)); - return xt::eval(xt::where(theta <= 0.0, theta + M_PI, theta - M_PI)); + auto && theta = xt::eval(xt::fmod(angles + M_PIF, 2.0f * M_PIF)); + return xt::eval(xt::where(theta <= 0.0f, theta + M_PIF, theta - M_PIF)); } /** @@ -310,13 +313,12 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data) size_t max_id_by_trajectories = 0, min_id_by_path = 0; float min_distance_by_path = std::numeric_limits::max(); - float cur_dist = 0.0f; for (size_t i = 0; i < dists.shape(0); i++) { min_id_by_path = 0; min_distance_by_path = std::numeric_limits::max(); - for (size_t j = 0; j < dists.shape(1); j++) { - cur_dist = dists(i, j); + for (size_t j = max_id_by_trajectories; j < dists.shape(1); j++) { + const float & cur_dist = dists(i, j); if (cur_dist < min_distance_by_path) { min_distance_by_path = cur_dist; min_id_by_path = j; @@ -436,7 +438,7 @@ inline float posePointAngle( if (!forward_preference) { return std::min( fabs(angles::shortest_angular_distance(yaw, pose_yaw)), - fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI)))); + fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PIF)))); } return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); @@ -454,14 +456,14 @@ inline float posePointAngle( const geometry_msgs::msg::Pose & pose, double point_x, double point_y, double point_yaw) { - float pose_x = pose.position.x; - float pose_y = pose.position.y; - float pose_yaw = tf2::getYaw(pose.orientation); + float pose_x = static_cast(pose.position.x); + float pose_y = static_cast(pose.position.y); + float pose_yaw = static_cast(tf2::getYaw(pose.orientation)); - float yaw = atan2f(point_y - pose_y, point_x - pose_x); + float yaw = atan2f(static_cast(point_y) - pose_y, static_cast(point_x) - pose_x); - if (fabs(angles::shortest_angular_distance(yaw, point_yaw)) > M_PI_2) { - yaw = angles::normalize_angle(yaw + M_PI); + if (fabs(angles::shortest_angular_distance(yaw, static_cast(point_yaw))) > M_PIF_2) { + yaw = angles::normalize_angle(yaw + M_PIF); } return fabs(angles::shortest_angular_distance(yaw, pose_yaw)); @@ -664,7 +666,7 @@ inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) // Checking for the existance of cusp, in the path, using the dot product. float dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { + if (dot_product < 0.0f) { return idx + 1; } } diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 2a7a77a234..2065c5b241 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -67,11 +67,11 @@ std::string CriticManager::getFullName(const std::string & name) void CriticManager::evalTrajectoriesScores( CriticData & data) const { - for (size_t q = 0; q < critics_.size(); q++) { + for (const auto & critic : critics_) { if (data.fail_flag) { break; } - critics_[q]->score(data); + critic->score(data); } } diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index e9774c7775..413ff75583 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -23,17 +23,17 @@ void ConstraintCritic::initialize() auto getParentParam = parameters_handler_->getParamGetter(parent_name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 4.0); + getParam(weight_, "cost_weight", 4.0f); RCLCPP_INFO( logger_, "ConstraintCritic instantiated with %d power and %f weight.", power_, weight_); float vx_max, vy_max, vx_min; - getParentParam(vx_max, "vx_max", 0.5); - getParentParam(vy_max, "vy_max", 0.0); - getParentParam(vx_min, "vx_min", -0.35); + getParentParam(vx_max, "vx_max", 0.5f); + getParentParam(vy_max, "vy_max", 0.0f); + getParentParam(vx_min, "vx_min", -0.35f); - const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; + const float min_sgn = vx_min > 0.0f ? 1.0f : -1.0f; max_vel_ = sqrtf(vx_max * vx_max + vy_max * vy_max); min_vel_ = min_sgn * sqrtf(vx_min * vx_min + vy_max * vy_max); } @@ -46,32 +46,46 @@ void ConstraintCritic::score(CriticData & data) return; } - auto sgn = xt::where(data.state.vx > 0.0, 1.0, -1.0); - auto vel_total = sgn * xt::sqrt(data.state.vx * data.state.vx + data.state.vy * data.state.vy); - auto out_of_max_bounds_motion = xt::maximum(vel_total - max_vel_, 0); - auto out_of_min_bounds_motion = xt::maximum(min_vel_ - vel_total, 0); + // Differential motion model + auto diff = dynamic_cast(data.motion_model.get()); + if (diff != nullptr) { + data.costs += xt::pow( + xt::sum( + (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) + + std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f))) * + data.model_dt, {1}, immediate) * weight_, power_); + return; + } + + // Omnidirectional motion model + auto omni = dynamic_cast(data.motion_model.get()); + if (omni != nullptr) { + auto sgn = xt::eval(xt::where(data.state.vx > 0.0f, 1.0f, -1.0f)); + auto vel_total = sgn * xt::hypot(data.state.vx, data.state.vy); + data.costs += xt::pow( + xt::sum( + (std::move(xt::maximum(vel_total - max_vel_, 0.0f)) + + std::move(xt::maximum(min_vel_ - vel_total, 0.0f))) * + data.model_dt, {1}, immediate) * weight_, power_); + return; + } + // Ackermann motion model auto acker = dynamic_cast(data.motion_model.get()); if (acker != nullptr) { auto & vx = data.state.vx; auto & wz = data.state.wz; auto out_of_turning_rad_motion = xt::maximum( - acker->getMinTurningRadius() - (xt::fabs(vx) / xt::fabs(wz)), 0.0); + acker->getMinTurningRadius() - (xt::fabs(vx) / xt::fabs(wz)), 0.0f); data.costs += xt::pow( xt::sum( - (std::move(out_of_max_bounds_motion) + - std::move(out_of_min_bounds_motion) + + (std::move(xt::maximum(data.state.vx - max_vel_, 0.0f)) + + std::move(xt::maximum(min_vel_ - data.state.vx, 0.0f)) + std::move(out_of_turning_rad_motion)) * data.model_dt, {1}, immediate) * weight_, power_); return; } - - data.costs += xt::pow( - xt::sum( - (std::move(out_of_max_bounds_motion) + - std::move(out_of_min_bounds_motion)) * - data.model_dt, {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 7a9b7729ed..d22de10722 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -24,10 +24,10 @@ void CostCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 3.81); - getParam(critical_cost_, "critical_cost", 300.0); - getParam(collision_cost_, "collision_cost", 1000000.0); - getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(weight_, "cost_weight", 3.81f); + getParam(critical_cost_, "critical_cost", 300.0f); + getParam(collision_cost_, "collision_cost", 1000000.0f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); // Normalized by cost value to put in same regime as other weights @@ -94,6 +94,8 @@ void CostCritic::score(CriticData & data) return; } + is_tracking_unknown_ = costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); + if (consider_footprint_) { // footprint may have changed since initialization if user has dynamic footprints possible_collision_cost_ = findCircumscribedCost(costmap_ros_); @@ -106,23 +108,34 @@ void CostCritic::score(CriticData & data) } auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - repulsive_cost.fill(0.0); + repulsive_cost.fill(0.0f); + + unsigned int x_i = 0u, y_i = 0u; + float pose_cost; + auto costmap = collision_checker_.getCostmap(); const size_t traj_len = data.trajectories.x.shape(1); bool all_trajectories_collide = true; for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) { bool trajectory_collide = false; - const auto & traj = data.trajectories; - float pose_cost; + const auto & traj_x = xt::view(data.trajectories.x, i, xt::all()); + const auto & traj_y = xt::view(data.trajectories.y, i, xt::all()); + const auto & traj_yaw = xt::view(data.trajectories.yaws, i, xt::all()); + pose_cost = 0.0f; for (size_t j = 0; j < traj_len; j++) { - // The costAtPose doesn't use orientation + // The getCost doesn't use orientation // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it // So the center point has more information than the footprint - pose_cost = costAtPose(traj.x(i, j), traj.y(i, j)); + if (!costmap->worldToMap(traj_x(j), traj_y(j), x_i, y_i)) { + pose_cost = static_cast(nav2_costmap_2d::NO_INFORMATION); + } else { + pose_cost = static_cast(costmap->getCost(x_i, y_i)); + } + if (pose_cost < 1.0f) {continue;} // In free space - if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) { + if (inCollision(pose_cost, traj_x(j), traj_y(j), traj_yaw(j))) { trajectory_collide = true; break; } @@ -130,8 +143,7 @@ void CostCritic::score(CriticData & data) // Let near-collision trajectory points be punished severely // Note that we collision check based on the footprint actual, // but score based on the center-point cost regardless - using namespace nav2_costmap_2d; // NOLINT - if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) { + if (pose_cost >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { repulsive_cost[i] += critical_cost_; } else if (!near_goal) { // Generally prefer trajectories further from obstacles repulsive_cost[i] += pose_cost; @@ -156,9 +168,6 @@ void CostCritic::score(CriticData & data) */ bool CostCritic::inCollision(float cost, float x, float y, float theta) { - bool is_tracking_unknown = - costmap_ros_->getLayeredCostmap()->isTrackingUnknown(); - // If consider_footprint_ check footprint scort for collision if (consider_footprint_ && (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) @@ -168,29 +177,17 @@ bool CostCritic::inCollision(float cost, float x, float y, float theta) } switch (static_cast(cost)) { - using namespace nav2_costmap_2d; // NOLINT - case (LETHAL_OBSTACLE): + case (nav2_costmap_2d::LETHAL_OBSTACLE): return true; - case (INSCRIBED_INFLATED_OBSTACLE): + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): return consider_footprint_ ? false : true; - case (NO_INFORMATION): - return is_tracking_unknown ? false : true; + case (nav2_costmap_2d::NO_INFORMATION): + return is_tracking_unknown_ ? false : true; } return false; } -float CostCritic::costAtPose(float x, float y) -{ - using namespace nav2_costmap_2d; // NOLINT - unsigned int x_i, y_i; - if (!collision_checker_.worldToMap(x, y, x_i, y_i)) { - return nav2_costmap_2d::NO_INFORMATION; - } - - return collision_checker_.pointCost(x_i, y_i); -} - } // namespace mppi::critics #include diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 604ee24eb1..ce8d4677a4 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -22,9 +22,9 @@ void GoalAngleCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 3.0); + getParam(weight_, "cost_weight", 3.0f); - getParam(threshold_to_consider_, "threshold_to_consider", 0.5); + getParam(threshold_to_consider_, "threshold_to_consider", 0.5f); RCLCPP_INFO( logger_, diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index f61d6fd2b2..d9d2ff97d1 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -25,8 +25,8 @@ void GoalCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0); - getParam(threshold_to_consider_, "threshold_to_consider", 1.4); + getParam(weight_, "cost_weight", 5.0f); + getParam(threshold_to_consider_, "threshold_to_consider", 1.4f); RCLCPP_INFO( logger_, "GoalCritic instantiated with %d power and %f weight.", @@ -49,11 +49,9 @@ void GoalCritic::score(CriticData & data) const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all()); const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all()); - auto dists = xt::sqrt( - xt::pow(traj_x - goal_x, 2) + - xt::pow(traj_y - goal_y, 2)); - - data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_); + data.costs += xt::pow( + xt::mean(xt::hypot(traj_x - goal_x, traj_y - goal_y), + {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index cdf0113564..6c477e5a53 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -24,11 +24,11 @@ void ObstaclesCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(consider_footprint_, "consider_footprint", false); getParam(power_, "cost_power", 1); - getParam(repulsion_weight_, "repulsion_weight", 1.5); - getParam(critical_weight_, "critical_weight", 20.0); - getParam(collision_cost_, "collision_cost", 100000.0); - getParam(collision_margin_distance_, "collision_margin_distance", 0.10); - getParam(near_goal_distance_, "near_goal_distance", 0.5); + getParam(repulsion_weight_, "repulsion_weight", 1.5f); + getParam(critical_weight_, "critical_weight", 20.0f); + getParam(collision_cost_, "collision_cost", 100000.0f); + getParam(collision_margin_distance_, "collision_margin_distance", 0.10f); + getParam(near_goal_distance_, "near_goal_distance", 0.5f); getParam(inflation_layer_name_, "inflation_layer_name", std::string("")); collision_checker_.setCostmap(costmap_); diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index cd913e5b0e..e76fba1c8c 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -27,14 +27,14 @@ void PathAlignCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 10.0); + getParam(weight_, "cost_weight", 10.0f); - getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07); + getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07f); getParam(offset_from_furthest_, "offset_from_furthest", 20); getParam(trajectory_point_step_, "trajectory_point_step", 4); getParam( threshold_to_consider_, - "threshold_to_consider", 0.5); + "threshold_to_consider", 0.5f); getParam(use_path_orientations_, "use_path_orientations", false); RCLCPP_INFO( @@ -61,11 +61,12 @@ void PathAlignCritic::score(CriticData & data) // Don't apply when dynamic obstacles are blocking significant proportions of the local path utils::setPathCostsIfNotSet(data, costmap_ros_); + std::vector path_pts_valid = *data.path_pts_valid; const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data); unsigned int invalid_ctr = 0; const float range = *data.furthest_reached_path_point - closest_initial_path_point; for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) { - if (!(*data.path_pts_valid)[i]) {invalid_ctr++;} + if (!path_pts_valid[i]) {invalid_ctr++;} if (static_cast(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) { return; } @@ -111,13 +112,12 @@ void PathAlignCritic::score(CriticData & data) // The nearest path point to align to needs to be not in collision, else // let the obstacle critic take over in this region due to dynamic obstacles - if ((*data.path_pts_valid)[path_pt]) { + if (path_pts_valid[path_pt]) { dx = P_x(path_pt) - Tx; dy = P_y(path_pt) - Ty; num_samples += 1.0f; if (use_path_orientations_) { - const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all()); - dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p)); + dyaw = angles::shortest_angular_distance(P_yaw(path_pt), data.trajectories.yaws(t, p)); summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw); } else { summed_path_dist += sqrtf(dx * dx + dy * dy); diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index d334b81673..be86e42779 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -27,22 +27,22 @@ void PathAngleCritic::initialize() auto getParentParam = parameters_handler_->getParamGetter(parent_name_); float vx_min; getParentParam(vx_min, "vx_min", -0.35); - if (fabs(vx_min) < 1e-6) { // zero + if (fabs(vx_min) < 1e-6f) { // zero reversing_allowed_ = false; - } else if (vx_min < 0.0) { // reversing possible + } else if (vx_min < 0.0f) { // reversing possible reversing_allowed_ = true; } auto getParam = parameters_handler_->getParamGetter(name_); getParam(offset_from_furthest_, "offset_from_furthest", 4); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 2.2); + getParam(weight_, "cost_weight", 2.2f); getParam( threshold_to_consider_, - "threshold_to_consider", 0.5); + "threshold_to_consider", 0.5f); getParam( max_angle_to_furthest_, - "max_angle_to_furthest", 0.785398); + "max_angle_to_furthest", 0.785398f); int mode = 0; getParam(mode, "mode", mode); @@ -114,7 +114,7 @@ void PathAngleCritic::score(CriticData & data) case PathAngleMode::NO_DIRECTIONAL_PREFERENCE: { const auto yaws_between_points_corrected = xt::where( - yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + yaws < M_PIF_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PIF)); const auto corrected_yaws = xt::abs( utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); @@ -123,8 +123,8 @@ 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_PI_2, - yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI)); + xt::abs(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( utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected)); data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_); diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 01ecb91728..a221cc939e 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -26,10 +26,10 @@ void PathFollowCritic::initialize() getParam( threshold_to_consider_, - "threshold_to_consider", 1.4); + "threshold_to_consider", 1.4f); getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0); + getParam(weight_, "cost_weight", 5.0f); } void PathFollowCritic::score(CriticData & data) @@ -63,11 +63,7 @@ void PathFollowCritic::score(CriticData & data) const auto last_x = xt::view(data.trajectories.x, xt::all(), -1); const auto last_y = xt::view(data.trajectories.y, xt::all(), -1); - auto dists = xt::sqrt( - xt::pow(last_x - path_x, 2) + - xt::pow(last_y - path_y, 2)); - - data.costs += xt::pow(weight_ * std::move(dists), power_); + data.costs += xt::pow(weight_ * std::move(xt::hypot(last_x - path_x, last_y - path_y)), power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 08e755dd6a..c873911bde 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -21,10 +21,10 @@ void PreferForwardCritic::initialize() { auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 5.0); + getParam(weight_, "cost_weight", 5.0f); getParam( threshold_to_consider_, - "threshold_to_consider", 0.5); + "threshold_to_consider", 0.5f); RCLCPP_INFO( logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_); @@ -39,11 +39,10 @@ void PreferForwardCritic::score(CriticData & data) return; } - auto backward_motion = xt::maximum(-data.state.vx, 0); data.costs += xt::pow( xt::sum( std::move( - backward_motion) * data.model_dt, {1}, immediate) * weight_, power_); + xt::maximum(-data.state.vx, 0)) * data.model_dt, {1}, immediate) * weight_, power_); } } // namespace mppi::critics diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 6b492d13ba..af677325ad 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -22,7 +22,7 @@ void TwirlingCritic::initialize() auto getParam = parameters_handler_->getParamGetter(name_); getParam(power_, "cost_power", 1); - getParam(weight_, "cost_weight", 10.0); + getParam(weight_, "cost_weight", 10.0f); RCLCPP_INFO( logger_, "TwirlingCritic instantiated with %d power and %f weight.", power_, weight_); @@ -37,8 +37,7 @@ void TwirlingCritic::score(CriticData & data) return; } - const auto wz = xt::abs(data.state.wz); - data.costs += xt::pow(xt::mean(wz, {1}, immediate) * weight_, power_); + data.costs += xt::pow(xt::mean(xt::abs(data.state.wz), {1}, immediate) * weight_, power_); } } // namespace mppi::critics