From b0a68bb5ef7df8ae3347358f8e453a688d423a55 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 19 Sep 2023 09:30:33 -0700 Subject: [PATCH] some minor optimizations (#3821) --- nav2_mppi_controller/CMakeLists.txt | 1 + .../critics/obstacles_critic.hpp | 6 +++--- .../src/critics/constraint_critic.cpp | 4 ++-- .../src/critics/obstacles_critic.cpp | 8 ++++---- .../src/critics/path_align_critic.cpp | 6 +++--- nav2_mppi_controller/src/optimizer.cpp | 14 +++++++------- 6 files changed, 20 insertions(+), 19 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 4abc9c7454d..9c3e5f6fa30 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -10,6 +10,7 @@ set(XTENSOR_USE_OPENMP 0) find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) +find_package(xsimd REQUIRED) set(dependencies_pkgs rclcpp diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp index a026b0fb2a9..37e14cb02c6 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -54,7 +54,7 @@ class ObstaclesCritic : public CriticFunction * @param cost Costmap cost * @return bool if in collision */ - bool inCollision(float cost) const; + inline bool inCollision(float cost) const; /** * @brief cost at a robot pose @@ -63,14 +63,14 @@ class ObstaclesCritic : public CriticFunction * @param theta theta of pose * @return Collision information at pose */ - CollisionCost costAtPose(float x, float y, float theta); + inline CollisionCost costAtPose(float x, float y, float theta); /** * @brief Distance to obstacle from cost * @param cost Costmap cost * @return float Distance to the obstacle represented by cost */ - float distanceToObstacle(const CollisionCost & cost); + inline float distanceToObstacle(const CollisionCost & cost); /** * @brief Find the min cost of the inflation decay function for which the robot MAY be diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp index 4dc2bcf1deb..e9774c7775f 100644 --- a/nav2_mppi_controller/src/critics/constraint_critic.cpp +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -34,8 +34,8 @@ void ConstraintCritic::initialize() getParentParam(vx_min, "vx_min", -0.35); const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; - max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); - min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_max * vy_max); + max_vel_ = sqrtf(vx_max * vx_max + vy_max * vy_max); + min_vel_ = min_sgn * sqrtf(vx_min * vx_min + vy_max * vy_max); } void ConstraintCritic::score(CriticData & data) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 60383d00a00..b9225635971 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -115,21 +115,21 @@ void ObstaclesCritic::score(CriticData & data) } auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - raw_cost.fill(0.0); + raw_cost.fill(0.0f); auto && repulsive_cost = xt::xtensor::from_shape({data.costs.shape(0)}); - repulsive_cost.fill(0.0); + repulsive_cost.fill(0.0f); 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; - float traj_cost = 0.0; + float traj_cost = 0.0f; const auto & traj = data.trajectories; CollisionCost pose_cost; for (size_t j = 0; j < traj_len; j++) { pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j)); - if (pose_cost.cost < 1) {continue;} // In free space + if (pose_cost.cost < 1.0f) {continue;} // In free space if (inCollision(pose_cost.cost)) { trajectory_collide = true; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 2585193ade5..795f166c1ad 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -88,12 +88,12 @@ void PathAlignCritic::score(CriticData & data) return; } - float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; + float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f; double min_dist_sq = std::numeric_limits::max(); size_t min_s = 0; for (size_t t = 0; t < batch_size; ++t) { - summed_dist = 0; + summed_dist = 0.0f; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { min_dist_sq = std::numeric_limits::max(); min_s = 0; @@ -118,7 +118,7 @@ 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 (min_s != 0 && (*data.path_pts_valid)[min_s]) { - summed_dist += std::sqrt(min_dist_sq); + summed_dist += sqrtf(min_dist_sq); } } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 55362fd7a8a..aae95015e1b 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -286,8 +286,8 @@ void Optimizer::integrateStateVelocities( const auto yaw_offseted = xt::view(traj_yaws, xt::range(1, _)); - xt::noalias(xt::view(yaw_cos, 0)) = std::cos(initial_yaw); - xt::noalias(xt::view(yaw_sin, 0)) = std::sin(initial_yaw); + xt::noalias(xt::view(yaw_cos, 0)) = cosf(initial_yaw); + xt::noalias(xt::view(yaw_sin, 0)) = sinf(initial_yaw); xt::noalias(xt::view(yaw_cos, xt::range(1, _))) = xt::cos(yaw_offseted); xt::noalias(xt::view(yaw_sin, xt::range(1, _))) = xt::sin(yaw_offseted); @@ -316,8 +316,8 @@ void Optimizer::integrateStateVelocities( auto && yaw_cos = xt::xtensor::from_shape(trajectories.yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(trajectories.yaws.shape()); - xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = std::cos(initial_yaw); - xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = std::sin(initial_yaw); + xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = cosf(initial_yaw); + xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = sinf(initial_yaw); xt::noalias(xt::view(yaw_cos, xt::all(), xt::range(1, _))) = xt::cos(yaws_cutted); xt::noalias(xt::view(yaw_sin, xt::all(), xt::range(1, _))) = xt::sin(yaws_cutted); @@ -358,16 +358,16 @@ void Optimizer::updateControlSequence() auto bounded_noises_vx = state_.cvx - control_sequence_.vx; auto bounded_noises_wz = state_.cwz - control_sequence_.wz; xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.vx, 2) * xt::sum( + s.gamma / powf(s.sampling_std.vx, 2) * xt::sum( xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate); xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.wz, 2) * xt::sum( + s.gamma / powf(s.sampling_std.wz, 2) * xt::sum( xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate); if (isHolonomic()) { auto bounded_noises_vy = state_.cvy - control_sequence_.vy; xt::noalias(costs_) += - s.gamma / std::pow(s.sampling_std.vy, 2) * xt::sum( + s.gamma / powf(s.sampling_std.vy, 2) * xt::sum( xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy, 1, immediate); }