25% speed up of goal critic; 1% speed up from vy striding when not in use

Signed-off-by: enricosutera <enricosutera@outlook.com>

Author: SteveMacenski

nav2_mppi_controller/src/critics/goal_critic.cpp

@@ -57,7 +57,7 @@ void GoalCritic::score(CriticData & data)
     xt::pow(traj_x - goal_x, 2) +
     xt::pow(traj_y - goal_y, 2));
 
-  data.costs += xt::pow(xt::mean(dists, {1}) * weight_, power_);
+  data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_);
 }
 
 }  // namespace mppi::critics

nav2_mppi_controller/src/optimizer.cpp

@@ -378,8 +378,10 @@ void Optimizer::updateControlSequence()
   auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis()));
 
   xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate);
-  xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
   xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate);
+  if (isHolonomic()) {
+    xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
+  }
 
   applyControlSequenceConstraints();
 }