diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 66780d21f3..b1c6585623 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -196,7 +196,7 @@ class Optimizer * @param state fill state */ void integrateStateVelocities( - Eigen::Array & trajectories, + Eigen::Array & trajectories, const Eigen::ArrayXXf & state) const; /** diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index de08cf3b34..a704ef8b5c 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -82,9 +82,9 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h // Recompute the noises on reset, initialization, and fallback { std::unique_lock guard(noise_lock_); - noises_vx_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); - noises_vy_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); - noises_wz_ = Eigen::ArrayXXf::Zero(settings_.batch_size, settings_.time_steps); + noises_vx_.setZero(settings_.batch_size, settings_.time_steps); + noises_vy_.setZero(settings_.batch_size, settings_.time_steps); + noises_wz_.setZero(settings_.batch_size, settings_.time_steps); ready_ = true; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3e25947287..5009db7504 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -300,7 +300,7 @@ void Optimizer::propagateStateVelocitiesFromInitials( } void Optimizer::integrateStateVelocities( - Eigen::Array & trajectory, + Eigen::Array & trajectory, const Eigen::ArrayXXf & sequence) const { float initial_yaw = static_cast(tf2::getYaw(state_.pose.pose.orientation)); @@ -386,8 +386,8 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory() { const bool is_holo = isHolonomic(); Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2); - Eigen::Array trajectories = - Eigen::Array(settings_.time_steps, 3); + Eigen::Array trajectories = + Eigen::Array(settings_.time_steps, 3); sequence.col(0) = control_sequence_.vx; sequence.col(1) = control_sequence_.wz;