some minor optimizations to MPPI

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

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

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)

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -115,21 +115,21 @@ void ObstaclesCritic::score(CriticData & data)
   }
 
   auto && raw_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
-  raw_cost.fill(0.0);
+  raw_cost.fill(0.0f);
   auto && repulsive_cost = xt::xtensor<float, 1>::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;

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<float>::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<float>::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);
       }
     }
 

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<float, 2>::from_shape(trajectories.yaws.shape());
   auto && yaw_sin = xt::xtensor<float, 2>::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);
   }