add humble-navigation2

2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
@@ -0,0 +1,138 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <stdint.h>
+#include <chrono>
+#include "nav2_mppi_controller/controller.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+// #define BENCHMARK_TESTING
+
+namespace nav2_mppi_controller
+{
+
+void MPPIController::configure(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+  std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
+  const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
+{
+  parent_ = parent;
+  costmap_ros_ = costmap_ros;
+  tf_buffer_ = tf;
+  name_ = name;
+  parameters_handler_ = std::make_unique<ParametersHandler>(parent);
+
+  auto node = parent_.lock();
+  clock_ = node->get_clock();
+  last_time_called_ = clock_->now();
+  // Get high-level controller parameters
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(visualize_, "visualize", false);
+  getParam(reset_period_, "reset_period", 1.0);
+
+  // Configure composed objects
+  optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get());
+  path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get());
+  trajectory_visualizer_.on_configure(
+    parent_, name_,
+    costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
+
+  RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::cleanup()
+{
+  optimizer_.shutdown();
+  trajectory_visualizer_.on_cleanup();
+  parameters_handler_.reset();
+  RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::activate()
+{
+  trajectory_visualizer_.on_activate();
+  parameters_handler_->start();
+  RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::deactivate()
+{
+  trajectory_visualizer_.on_deactivate();
+  RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
+}
+
+void MPPIController::reset()
+{
+  optimizer_.reset();
+}
+
+geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  nav2_core::GoalChecker * goal_checker)
+{
+#ifdef BENCHMARK_TESTING
+  auto start = std::chrono::system_clock::now();
+#endif
+
+  if (clock_->now() - last_time_called_ > rclcpp::Duration::from_seconds(reset_period_)) {
+    reset();
+  }
+  last_time_called_ = clock_->now();
+
+  std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
+  geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose;
+
+  nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
+
+  nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
+  std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
+
+  geometry_msgs::msg::TwistStamped cmd =
+    optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
+
+#ifdef BENCHMARK_TESTING
+  auto end = std::chrono::system_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+  RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
+#endif
+
+  if (visualize_) {
+    visualize(std::move(transformed_plan));
+  }
+
+  return cmd;
+}
+
+void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
+{
+  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
+  trajectory_visualizer_.visualize(std::move(transformed_plan));
+}
+
+void MPPIController::setPlan(const nav_msgs::msg::Path & path)
+{
+  path_handler_.setPath(path);
+}
+
+void MPPIController::setSpeedLimit(const double & speed_limit, const bool & percentage)
+{
+  optimizer_.setSpeedLimit(speed_limit, percentage);
+}
+
+}  // namespace nav2_mppi_controller
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller)
@@ -0,0 +1,78 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critic_manager.hpp"
+
+namespace mppi
+{
+
+void CriticManager::on_configure(
+  rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros, ParametersHandler * param_handler)
+{
+  parent_ = parent;
+  costmap_ros_ = costmap_ros;
+  name_ = name;
+  auto node = parent_.lock();
+  logger_ = node->get_logger();
+  parameters_handler_ = param_handler;
+
+  getParams();
+  loadCritics();
+}
+
+void CriticManager::getParams()
+{
+  auto node = parent_.lock();
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(critic_names_, "critics", std::vector<std::string>{}, ParameterType::Static);
+}
+
+void CriticManager::loadCritics()
+{
+  if (!loader_) {
+    loader_ = std::make_unique<pluginlib::ClassLoader<critics::CriticFunction>>(
+      "nav2_mppi_controller", "mppi::critics::CriticFunction");
+  }
+
+  critics_.clear();
+  for (auto name : critic_names_) {
+    std::string fullname = getFullName(name);
+    auto instance = std::unique_ptr<critics::CriticFunction>(
+      loader_->createUnmanagedInstance(fullname));
+    critics_.push_back(std::move(instance));
+    critics_.back()->on_configure(
+      parent_, name_, name_ + "." + name, costmap_ros_,
+      parameters_handler_);
+    RCLCPP_INFO(logger_, "Critic loaded : %s", fullname.c_str());
+  }
+}
+
+std::string CriticManager::getFullName(const std::string & name)
+{
+  return "mppi::critics::" + name;
+}
+
+void CriticManager::evalTrajectoriesScores(
+  CriticData & data) const
+{
+  for (size_t q = 0; q < critics_.size(); q++) {
+    if (data.fail_flag) {
+      break;
+    }
+    critics_[q]->score(data);
+  }
+}
+
+}  // namespace mppi
@@ -0,0 +1,81 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/constraint_critic.hpp"
+
+namespace mppi::critics
+{
+
+void ConstraintCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
+
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 4.0);
+  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);
+
+  const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0;
+  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)
+{
+  using xt::evaluation_strategy::immediate;
+
+  if (!enabled_) {
+    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);
+
+  auto acker = dynamic_cast<AckermannMotionModel *>(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);
+
+    data.costs += xt::pow(
+      xt::sum(
+        (std::move(out_of_max_bounds_motion) +
+        std::move(out_of_min_bounds_motion) +
+        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
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(mppi::critics::ConstraintCritic, mppi::critics::CriticFunction)
@@ -0,0 +1,231 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/exceptions.hpp"
+
+namespace mppi::critics
+{
+
+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(inflation_layer_name_, "inflation_layer_name", std::string(""));
+
+  // Normalized by cost value to put in same regime as other weights
+  weight_ /= 254.0f;
+
+  // Normalize weight when parameter is changed dynamically as well
+  auto weightDynamicCb = [&](const rclcpp::Parameter & weight) {
+      weight_ = weight.as_double() / 254.0f;
+    };
+  parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb);
+
+  collision_checker_.setCostmap(costmap_);
+  possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possibly_inscribed_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+      logger_,
+      "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+      "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::PlannerException(
+              "Considering footprint in collision checking but no robot footprint provided in the "
+              "costmap.");
+    }
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "InflationCostCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_cost_, weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float CostCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  bool inflation_layer_found = false;
+  double result = -1.0;
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
+    layer != costmap->getLayeredCostmap()->getPlugins()->end();
+    ++layer)
+  {
+    auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
+    if (!inflation_layer ||
+      (!inflation_layer_name_.empty() &&
+      inflation_layer->getName() != inflation_layer_name_))
+    {
+      continue;
+    }
+
+    inflation_layer_found = true;
+    const double resolution = costmap->getCostmap()->getResolution();
+    result = inflation_layer->computeCost(circum_radius / resolution);
+  }
+
+  if (!inflation_layer_found) {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+void CostCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+  if (!enabled_) {
+    return;
+  }
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  // If near the goal, don't apply the preferential term since the goal is near obstacles
+  bool near_goal = false;
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
+    near_goal = true;
+  }
+
+  auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
+  repulsive_cost.fill(0.0);
+
+  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;
+
+    for (size_t j = 0; j < traj_len; j++) {
+      // The costAtPose 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 (pose_cost < 1.0f) {continue;}  // In free space
+
+      if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) {
+        trajectory_collide = true;
+        break;
+      }
+
+      // 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) {
+        repulsive_cost[i] += critical_cost_;
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        repulsive_cost[i] += pose_cost;
+      }
+    }
+
+    if (!trajectory_collide) {
+      all_trajectories_collide = false;
+    } else {
+      repulsive_cost[i] = collision_cost_;
+    }
+  }
+
+  data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_);
+  data.fail_flag = all_trajectories_collide;
+}
+
+/**
+  * @brief Checks if cost represents a collision
+  * @param cost Costmap cost
+  * @return bool if in collision
+  */
+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 >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
+  {
+    cost = static_cast<float>(collision_checker_.footprintCostAtPose(
+        x, y, theta, costmap_ros_->getRobotFootprint()));
+  }
+
+  switch (static_cast<unsigned char>(cost)) {
+    using namespace nav2_costmap_2d; // NOLINT
+    case (LETHAL_OBSTACLE):
+      return true;
+    case (INSCRIBED_INFLATED_OBSTACLE):
+      return consider_footprint_ ? false : true;
+    case (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 <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::CostCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,58 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
+
+namespace mppi::critics
+{
+
+void GoalAngleCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 3.0);
+
+  getParam(threshold_to_consider_, "threshold_to_consider", 0.5);
+
+  RCLCPP_INFO(
+    logger_,
+    "GoalAngleCritic instantiated with %d power, %f weight, and %f "
+    "angular threshold.",
+    power_, weight_, threshold_to_consider_);
+}
+
+void GoalAngleCritic::score(CriticData & data)
+{
+  if (!enabled_ || !utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  const auto goal_idx = data.path.x.shape(0) - 1;
+  const float goal_yaw = data.path.yaws(goal_idx);
+
+  data.costs += xt::pow(
+    xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
+    weight_, power_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::GoalAngleCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,61 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/goal_critic.hpp"
+
+namespace mppi::critics
+{
+
+using xt::evaluation_strategy::immediate;
+
+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);
+
+  RCLCPP_INFO(
+    logger_, "GoalCritic instantiated with %d power and %f weight.",
+    power_, weight_);
+}
+
+void GoalCritic::score(CriticData & data)
+{
+  if (!enabled_ || !utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  const auto & goal_x = data.goal.position.x;
+  const auto & goal_y = data.goal.position.y;
+
+  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_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(mppi::critics::GoalCritic, mppi::critics::CriticFunction)
@@ -0,0 +1,245 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <cmath>
+#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
+#include "nav2_core/exceptions.hpp"
+
+namespace mppi::critics
+{
+
+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", 10000.0);
+  getParam(collision_margin_distance_, "collision_margin_distance", 0.10);
+  getParam(near_goal_distance_, "near_goal_distance", 0.5);
+
+  collision_checker_.setCostmap(costmap_);
+  possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possibly_inscribed_cost_ < 1.0f) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+      logger_,
+      "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+      "in both the costmap and the obstacle critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::PlannerException(
+              "Considering footprint in collision checking but no robot footprint provided in the "
+              "costmap.");
+    }
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "ObstaclesCritic instantiated with %d power and %f / %f weights. "
+    "Critic will collision check based on %s cost.",
+    power_, critical_weight_, repulsion_weight_, consider_footprint_ ?
+    "footprint" : "circular");
+}
+
+float ObstaclesCritic::findCircumscribedCost(
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
+{
+  double result = -1.0;
+  bool inflation_layer_found = false;
+
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
+  // check if the costmap has an inflation layer
+  for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
+    layer != costmap->getLayeredCostmap()->getPlugins()->end();
+    ++layer)
+  {
+    auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
+    if (!inflation_layer) {
+      continue;
+    }
+
+    inflation_layer_found = true;
+    const double resolution = costmap->getCostmap()->getResolution();
+    result = inflation_layer->computeCost(circum_radius / resolution);
+    auto getParam = parameters_handler_->getParamGetter(name_);
+    getParam(inflation_scale_factor_, "cost_scaling_factor", 10.0);
+    getParam(inflation_radius_, "inflation_radius", 0.55);
+  }
+
+  if (!inflation_layer_found) {
+    RCLCPP_WARN(
+      logger_,
+      "No inflation layer found in costmap configuration. "
+      "If this is an SE2-collision checking plugin, it cannot use costmap potential "
+      "field to speed up collision checking by only checking the full footprint "
+      "when robot is within possibly-inscribed radius of an obstacle. This may "
+      "significantly slow down planning times and not avoid anything but absolute collisions!");
+  }
+
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
+}
+
+float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
+{
+  const float scale_factor = inflation_scale_factor_;
+  const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
+  float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor;
+
+  // If not footprint collision checking, the cost is using the center point cost and
+  // needs the radius subtracted to obtain the closest distance to the object
+  if (!cost.using_footprint) {
+    dist_to_obj -= min_radius;
+  }
+
+  return dist_to_obj;
+}
+
+void ObstaclesCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+  if (!enabled_) {
+    return;
+  }
+
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
+  // If near the goal, don't apply the preferential term since the goal is near obstacles
+  bool near_goal = false;
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
+    near_goal = true;
+  }
+
+  auto && raw_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
+  raw_cost.fill(0.0f);
+  auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(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.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.0f) {continue;}  // In free space
+
+      if (inCollision(pose_cost.cost)) {
+        trajectory_collide = true;
+        break;
+      }
+
+      // Cannot process repulsion if inflation layer does not exist
+      if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {
+        continue;
+      }
+
+      const float dist_to_obj = distanceToObstacle(pose_cost);
+
+      // Let near-collision trajectory points be punished severely
+      if (dist_to_obj < collision_margin_distance_) {
+        traj_cost += (collision_margin_distance_ - dist_to_obj);
+      } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
+        repulsive_cost[i] += (inflation_radius_ - dist_to_obj);
+      }
+    }
+
+    if (!trajectory_collide) {all_trajectories_collide = false;}
+    raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost;
+  }
+
+  data.costs += xt::pow(
+    (critical_weight_ * raw_cost) +
+    (repulsion_weight_ * repulsive_cost / traj_len),
+    power_);
+  data.fail_flag = all_trajectories_collide;
+}
+
+/**
+  * @brief Checks if cost represents a collision
+  * @param cost Costmap cost
+  * @return bool if in collision
+  */
+bool ObstaclesCritic::inCollision(float cost) const
+{
+  bool is_tracking_unknown =
+    costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
+
+  switch (static_cast<unsigned char>(cost)) {
+    using namespace nav2_costmap_2d; // NOLINT
+    case (LETHAL_OBSTACLE):
+      return true;
+    case (INSCRIBED_INFLATED_OBSTACLE):
+      return consider_footprint_ ? false : true;
+    case (NO_INFORMATION):
+      return is_tracking_unknown ? false : true;
+  }
+
+  return false;
+}
+
+CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
+{
+  CollisionCost collision_cost;
+  float & cost = collision_cost.cost;
+  collision_cost.using_footprint = false;
+  unsigned int x_i, y_i;
+  if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
+    cost = nav2_costmap_2d::NO_INFORMATION;
+    return collision_cost;
+  }
+  cost = collision_checker_.pointCost(x_i, y_i);
+
+  if (consider_footprint_ &&
+    (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
+  {
+    cost = static_cast<float>(collision_checker_.footprintCostAtPose(
+        x, y, theta, costmap_ros_->getRobotFootprint()));
+    collision_cost.using_footprint = true;
+  }
+
+  return collision_cost;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::ObstaclesCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,144 @@
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/path_align_critic.hpp"
+
+#include <xtensor/xfixed.hpp>
+#include <xtensor/xmath.hpp>
+
+namespace mppi::critics
+{
+
+using namespace xt::placeholders;  // NOLINT
+using xt::evaluation_strategy::immediate;
+
+void PathAlignCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 10.0);
+
+  getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07);
+  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);
+  getParam(use_path_orientations_, "use_path_orientations", false);
+
+  RCLCPP_INFO(
+    logger_,
+    "ReferenceTrajectoryCritic instantiated with %d power and %f weight",
+    power_, weight_);
+}
+
+void PathAlignCritic::score(CriticData & data)
+{
+  // Don't apply close to goal, let the goal critics take over
+  if (!enabled_ || utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  // Don't apply when first getting bearing w.r.t. the path
+  utils::setPathFurthestPointIfNotSet(data);
+  const size_t path_segments_count = *data.furthest_reached_path_point;  // up to furthest only
+  if (path_segments_count < offset_from_furthest_) {
+    return;
+  }
+
+  // Don't apply when dynamic obstacles are blocking significant proportions of the local path
+  utils::setPathCostsIfNotSet(data, costmap_ros_);
+  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 (static_cast<float>(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) {
+      return;
+    }
+  }
+
+  const auto P_x = xt::view(data.path.x, xt::range(_, -1));  // path points
+  const auto P_y = xt::view(data.path.y, xt::range(_, -1));  // path points
+  const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1));  // path points
+
+  const size_t batch_size = data.trajectories.x.shape(0);
+  const size_t time_steps = data.trajectories.x.shape(1);
+  auto && cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
+
+  // Find integrated distance in the path
+  std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
+  float dx = 0.0f, dy = 0.0f;
+  for (unsigned int i = 1; i != path_segments_count; i++) {
+    dx = P_x(i) - P_x(i - 1);
+    dy = P_y(i) - P_y(i - 1);
+    float curr_dist = sqrtf(dx * dx + dy * dy);
+    path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist;
+  }
+
+  float traj_integrated_distance = 0.0f;
+  float summed_path_dist = 0.0f, dyaw = 0.0f;
+  float num_samples = 0.0f;
+  float Tx = 0.0f, Ty = 0.0f;
+  size_t path_pt = 0;
+  for (size_t t = 0; t < batch_size; ++t) {
+    traj_integrated_distance = 0.0f;
+    summed_path_dist = 0.0f;
+    num_samples = 0.0f;
+    path_pt = 0u;
+    const auto T_x = xt::view(data.trajectories.x, t, xt::all());
+    const auto T_y = xt::view(data.trajectories.y, t, xt::all());
+    for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) {
+      Tx = T_x(p);
+      Ty = T_y(p);
+      dx = Tx - T_x(p - trajectory_point_step_);
+      dy = Ty - T_y(p - trajectory_point_step_);
+      traj_integrated_distance += sqrtf(dx * dx + dy * dy);
+      path_pt = utils::findClosestPathPt(
+        path_integrated_distances, traj_integrated_distance, path_pt);
+
+      // 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]) {
+        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));
+          summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw);
+        } else {
+          summed_path_dist += sqrtf(dx * dx + dy * dy);
+        }
+      }
+    }
+    if (num_samples > 0) {
+      cost[t] = summed_path_dist / num_samples;
+    } else {
+      cost[t] = 0.0f;
+    }
+  }
+
+  data.costs += xt::pow(std::move(cost) * weight_, power_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::PathAlignCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,137 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp"
+
+#include <xtensor/xfixed.hpp>
+#include <xtensor/xmath.hpp>
+
+namespace mppi::critics
+{
+
+using namespace xt::placeholders;  // NOLINT
+using xt::evaluation_strategy::immediate;
+
+void PathAlignLegacyCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 10.0);
+
+  getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07);
+  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);
+  getParam(use_path_orientations_, "use_path_orientations", false);
+
+  RCLCPP_INFO(
+    logger_,
+    "ReferenceTrajectoryCritic instantiated with %d power and %f weight",
+    power_, weight_);
+}
+
+void PathAlignLegacyCritic::score(CriticData & data)
+{
+  // Don't apply close to goal, let the goal critics take over
+  if (!enabled_ || utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  // Don't apply when first getting bearing w.r.t. the path
+  utils::setPathFurthestPointIfNotSet(data);
+  if (*data.furthest_reached_path_point < offset_from_furthest_) {
+    return;
+  }
+
+  // Don't apply when dynamic obstacles are blocking significant proportions of the local path
+  utils::setPathCostsIfNotSet(data, costmap_ros_);
+  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 (static_cast<float>(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) {
+      return;
+    }
+  }
+
+  const auto & T_x = data.trajectories.x;
+  const auto & T_y = data.trajectories.y;
+  const auto & T_yaw = data.trajectories.yaws;
+
+  const auto P_x = xt::view(data.path.x, xt::range(_, -1));  // path points
+  const auto P_y = xt::view(data.path.y, xt::range(_, -1));  // path points
+  const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1));  // path points
+
+  const size_t batch_size = T_x.shape(0);
+  const size_t time_steps = T_x.shape(1);
+  const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_);
+  const size_t path_segments_count = data.path.x.shape(0) - 1;
+  auto && cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
+
+  if (path_segments_count < 1) {
+    return;
+  }
+
+  float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f;
+  float 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.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;
+
+      // Find closest path segment to the trajectory point
+      for (size_t s = 0; s < path_segments_count - 1; s++) {
+        xt::xtensor_fixed<float, xt::xshape<2>> P;
+        dx = P_x(s) - T_x(t, p);
+        dy = P_y(s) - T_y(t, p);
+        if (use_path_orientations_) {
+          dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p));
+          dist_sq = dx * dx + dy * dy + dyaw * dyaw;
+        } else {
+          dist_sq = dx * dx + dy * dy;
+        }
+        if (dist_sq < min_dist_sq) {
+          min_dist_sq = dist_sq;
+          min_s = s;
+        }
+      }
+
+      // 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 += sqrtf(min_dist_sq);
+      }
+    }
+
+    cost[t] = summed_dist / traj_pts_eval;
+  }
+
+  data.costs += xt::pow(std::move(cost) * weight_, power_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::PathAlignLegacyCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,109 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/path_angle_critic.hpp"
+
+#include <math.h>
+
+namespace mppi::critics
+{
+
+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
+    reversing_allowed_ = false;
+  } else if (vx_min < 0.0) {   // 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.0);
+  getParam(
+    threshold_to_consider_,
+    "threshold_to_consider", 0.5);
+  getParam(
+    max_angle_to_furthest_,
+    "max_angle_to_furthest", 1.2);
+  getParam(
+    forward_preference_,
+    "forward_preference", true);
+
+  if (!reversing_allowed_) {
+    forward_preference_ = true;
+  }
+
+  RCLCPP_INFO(
+    logger_,
+    "PathAngleCritic instantiated with %d power and %f weight. Reversing %s",
+    power_, weight_, reversing_allowed_ ? "allowed." : "not allowed.");
+}
+
+void PathAngleCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+  if (!enabled_) {
+    return;
+  }
+
+  if (utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  utils::setPathFurthestPointIfNotSet(data);
+
+  auto offseted_idx = std::min(
+    *data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1);
+
+  const float goal_x = xt::view(data.path.x, offseted_idx);
+  const float goal_y = xt::view(data.path.y, offseted_idx);
+
+  if (utils::posePointAngle(
+      data.state.pose.pose, goal_x, goal_y, forward_preference_) < max_angle_to_furthest_)
+  {
+    return;
+  }
+
+  auto yaws_between_points = xt::atan2(
+    goal_y - data.trajectories.y,
+    goal_x - data.trajectories.x);
+
+  auto yaws =
+    xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points));
+
+  if (reversing_allowed_ && !forward_preference_) {
+    const auto yaws_between_points_corrected = xt::where(
+      yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI));
+    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_);
+  } else {
+    data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_);
+  }
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::PathAngleCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,79 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/path_follow_critic.hpp"
+
+#include <xtensor/xmath.hpp>
+#include <xtensor/xsort.hpp>
+
+namespace mppi::critics
+{
+
+void PathFollowCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+
+  getParam(
+    threshold_to_consider_,
+    "threshold_to_consider", 1.4);
+  getParam(offset_from_furthest_, "offset_from_furthest", 6);
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 5.0);
+}
+
+void PathFollowCritic::score(CriticData & data)
+{
+  if (!enabled_ || data.path.x.shape(0) < 2 ||
+    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  utils::setPathFurthestPointIfNotSet(data);
+  utils::setPathCostsIfNotSet(data, costmap_ros_);
+  const size_t path_size = data.path.x.shape(0) - 1;
+
+  auto offseted_idx = std::min(
+    *data.furthest_reached_path_point + offset_from_furthest_, path_size);
+
+  // Drive to the first valid path point, in case of dynamic obstacles on path
+  // we want to drive past it, not through it
+  bool valid = false;
+  while (!valid && offseted_idx < path_size - 1) {
+    valid = (*data.path_pts_valid)[offseted_idx];
+    if (!valid) {
+      offseted_idx++;
+    }
+  }
+
+  const auto path_x = data.path.x(offseted_idx);
+  const auto path_y = data.path.y(offseted_idx);
+
+  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_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::PathFollowCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,55 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"
+
+namespace mppi::critics
+{
+
+void PreferForwardCritic::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", 0.5);
+
+  RCLCPP_INFO(
+    logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_);
+}
+
+void PreferForwardCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+  if (!enabled_ || utils::withinPositionGoalTolerance(
+      threshold_to_consider_, data.state.pose.pose, data.goal))
+  {
+    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_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::PreferForwardCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,50 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/twirling_critic.hpp"
+
+namespace mppi::critics
+{
+
+void TwirlingCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 10.0);
+
+  RCLCPP_INFO(
+    logger_, "TwirlingCritic instantiated with %d power and %f weight.", power_, weight_);
+}
+
+void TwirlingCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+  if (!enabled_ ||
+    utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal))
+  {
+    return;
+  }
+
+  const auto wz = xt::abs(data.state.wz);
+  data.costs += xt::pow(xt::mean(wz, {1}, immediate) * weight_, power_);
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  mppi::critics::TwirlingCritic,
+  mppi::critics::CriticFunction)
@@ -0,0 +1,104 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp"
+
+namespace mppi::critics
+{
+
+void VelocityDeadbandCritic::initialize()
+{
+  auto getParam = parameters_handler_->getParamGetter(name_);
+
+  getParam(power_, "cost_power", 1);
+  getParam(weight_, "cost_weight", 35.0);
+
+  // Recast double to float
+  std::vector<double> deadband_velocities{0.0, 0.0, 0.0};
+  getParam(deadband_velocities, "deadband_velocities", std::vector<double>{0.0, 0.0, 0.0});
+  std::transform(
+    deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(),
+    [](double d) {return static_cast<float>(d);});
+
+  RCLCPP_INFO_STREAM(
+    logger_, "VelocityDeadbandCritic instantiated with "
+      << power_ << " power, " << weight_ << " weight, deadband_velocity ["
+      << deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << ","
+      << deadband_velocities_.at(2) << "]");
+}
+
+void VelocityDeadbandCritic::score(CriticData & data)
+{
+  using xt::evaluation_strategy::immediate;
+
+  if (!enabled_) {
+    return;
+  }
+
+  auto & vx = data.state.vx;
+  auto & wz = data.state.wz;
+
+  if (data.motion_model->isHolonomic()) {
+    auto & vy = data.state.vy;
+    if (power_ > 1u) {
+      data.costs += xt::pow(
+        xt::sum(
+          std::move(
+            xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
+            xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) +
+            xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) *
+          data.model_dt,
+          {1}, immediate) *
+        weight_,
+        power_);
+    } else {
+      data.costs += xt::sum(
+        (std::move(
+          xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
+          xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) +
+          xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) *
+        data.model_dt,
+        {1}, immediate) *
+        weight_;
+    }
+    return;
+  }
+
+  if (power_ > 1u) {
+    data.costs += xt::pow(
+      xt::sum(
+        std::move(
+          xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
+          xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) *
+        data.model_dt,
+        {1}, immediate) *
+      weight_,
+      power_);
+  } else {
+    data.costs += xt::sum(
+      (std::move(
+        xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
+        xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) *
+      data.model_dt,
+      {1}, immediate) *
+      weight_;
+  }
+  return;
+}
+
+}  // namespace mppi::critics
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction)
@@ -0,0 +1,124 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/noise_generator.hpp"
+
+#include <memory>
+#include <mutex>
+#include <xtensor/xmath.hpp>
+#include <xtensor/xrandom.hpp>
+#include <xtensor/xnoalias.hpp>
+
+namespace mppi
+{
+
+void NoiseGenerator::initialize(
+  mppi::models::OptimizerSettings & settings, bool is_holonomic,
+  const std::string & name, ParametersHandler * param_handler)
+{
+  settings_ = settings;
+  is_holonomic_ = is_holonomic;
+  active_ = true;
+
+  auto getParam = param_handler->getParamGetter(name);
+  getParam(regenerate_noises_, "regenerate_noises", false);
+
+  if (regenerate_noises_) {
+    noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this));
+  } else {
+    generateNoisedControls();
+  }
+}
+
+void NoiseGenerator::shutdown()
+{
+  active_ = false;
+  ready_ = true;
+  noise_cond_.notify_all();
+  if (noise_thread_.joinable()) {
+    noise_thread_.join();
+  }
+}
+
+void NoiseGenerator::generateNextNoises()
+{
+  // Trigger the thread to run in parallel to this iteration
+  // to generate the next iteration's noises (if applicable).
+  {
+    std::unique_lock<std::mutex> guard(noise_lock_);
+    ready_ = true;
+  }
+  noise_cond_.notify_all();
+}
+
+void NoiseGenerator::setNoisedControls(
+  models::State & state,
+  const models::ControlSequence & control_sequence)
+{
+  std::unique_lock<std::mutex> guard(noise_lock_);
+
+  xt::noalias(state.cvx) = control_sequence.vx + noises_vx_;
+  xt::noalias(state.cvy) = control_sequence.vy + noises_vy_;
+  xt::noalias(state.cwz) = control_sequence.wz + noises_wz_;
+}
+
+void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic)
+{
+  settings_ = settings;
+  is_holonomic_ = is_holonomic;
+
+  // Recompute the noises on reset, initialization, and fallback
+  {
+    std::unique_lock<std::mutex> guard(noise_lock_);
+    xt::noalias(noises_vx_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
+    xt::noalias(noises_vy_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
+    xt::noalias(noises_wz_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
+    ready_ = true;
+  }
+
+  if (regenerate_noises_) {
+    noise_cond_.notify_all();
+  } else {
+    generateNoisedControls();
+  }
+}
+
+void NoiseGenerator::noiseThread()
+{
+  do {
+    std::unique_lock<std::mutex> guard(noise_lock_);
+    noise_cond_.wait(guard, [this]() {return ready_;});
+    ready_ = false;
+    generateNoisedControls();
+  } while (active_);
+}
+
+void NoiseGenerator::generateNoisedControls()
+{
+  auto & s = settings_;
+
+  xt::noalias(noises_vx_) = xt::random::randn<float>(
+    {s.batch_size, s.time_steps}, 0.0f,
+    s.sampling_std.vx);
+  xt::noalias(noises_wz_) = xt::random::randn<float>(
+    {s.batch_size, s.time_steps}, 0.0f,
+    s.sampling_std.wz);
+  if (is_holonomic_) {
+    xt::noalias(noises_vy_) = xt::random::randn<float>(
+      {s.batch_size, s.time_steps}, 0.0f,
+      s.sampling_std.vy);
+  }
+}
+
+}  // namespace mppi
@@ -0,0 +1,460 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/optimizer.hpp"
+
+#include <limits>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <cmath>
+#include <xtensor/xmath.hpp>
+#include <xtensor/xrandom.hpp>
+#include <xtensor/xnoalias.hpp>
+
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
+
+namespace mppi
+{
+
+using namespace xt::placeholders;  // NOLINT
+using xt::evaluation_strategy::immediate;
+
+void Optimizer::initialize(
+  rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+  ParametersHandler * param_handler)
+{
+  parent_ = parent;
+  name_ = name;
+  costmap_ros_ = costmap_ros;
+  costmap_ = costmap_ros_->getCostmap();
+  parameters_handler_ = param_handler;
+
+  auto node = parent_.lock();
+  logger_ = node->get_logger();
+
+  getParams();
+
+  critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_);
+  noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_);
+
+  reset();
+}
+
+void Optimizer::shutdown()
+{
+  noise_generator_.shutdown();
+}
+
+void Optimizer::getParams()
+{
+  std::string motion_model_name;
+
+  auto & s = settings_;
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  auto getParentParam = parameters_handler_->getParamGetter("");
+  getParam(s.model_dt, "model_dt", 0.05f);
+  getParam(s.time_steps, "time_steps", 56);
+  getParam(s.batch_size, "batch_size", 1000);
+  getParam(s.iteration_count, "iteration_count", 1);
+  getParam(s.temperature, "temperature", 0.3f);
+  getParam(s.gamma, "gamma", 0.015f);
+  getParam(s.base_constraints.vx_max, "vx_max", 0.5);
+  getParam(s.base_constraints.vx_min, "vx_min", -0.35);
+  getParam(s.base_constraints.vy, "vy_max", 0.5);
+  getParam(s.base_constraints.wz, "wz_max", 1.9);
+  getParam(s.sampling_std.vx, "vx_std", 0.2);
+  getParam(s.sampling_std.vy, "vy_std", 0.2);
+  getParam(s.sampling_std.wz, "wz_std", 0.4);
+  getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
+
+  getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
+
+  s.constraints = s.base_constraints;
+  setMotionModel(motion_model_name);
+  parameters_handler_->addPostCallback([this]() {reset();});
+
+  double controller_frequency;
+  getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static);
+  setOffset(controller_frequency);
+}
+
+void Optimizer::setOffset(double controller_frequency)
+{
+  const double controller_period = 1.0 / controller_frequency;
+  constexpr double eps = 1e-6;
+
+  if ((controller_period + eps) < settings_.model_dt) {
+    RCLCPP_WARN(
+      logger_,
+      "Controller period is less then model dt, consider setting it equal");
+  } else if (abs(controller_period - settings_.model_dt) < eps) {
+    RCLCPP_INFO(
+      logger_,
+      "Controller period is equal to model dt. Control sequence "
+      "shifting is ON");
+    settings_.shift_control_sequence = true;
+  } else {
+    throw std::runtime_error(
+            "Controller period more then model dt, set it equal to model dt");
+  }
+}
+
+void Optimizer::reset()
+{
+  state_.reset(settings_.batch_size, settings_.time_steps);
+  control_sequence_.reset(settings_.time_steps);
+  control_history_[0] = {0.0, 0.0, 0.0};
+  control_history_[1] = {0.0, 0.0, 0.0};
+  control_history_[2] = {0.0, 0.0, 0.0};
+  control_history_[3] = {0.0, 0.0, 0.0};
+
+  settings_.constraints = settings_.base_constraints;
+
+  costs_ = xt::zeros<float>({settings_.batch_size});
+  generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
+
+  noise_generator_.reset(settings_, isHolonomic());
+  RCLCPP_INFO(logger_, "Optimizer reset");
+}
+
+geometry_msgs::msg::TwistStamped Optimizer::evalControl(
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  const nav_msgs::msg::Path & plan,
+  const geometry_msgs::msg::Pose & goal,
+  nav2_core::GoalChecker * goal_checker)
+{
+  prepare(robot_pose, robot_speed, plan, goal, goal_checker);
+
+  do {
+    optimize();
+  } while (fallback(critics_data_.fail_flag));
+
+  utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
+  auto control = getControlFromSequenceAsTwist(plan.header.stamp);
+
+  if (settings_.shift_control_sequence) {
+    shiftControlSequence();
+  }
+
+  return control;
+}
+
+void Optimizer::optimize()
+{
+  for (size_t i = 0; i < settings_.iteration_count; ++i) {
+    generateNoisedTrajectories();
+    critic_manager_.evalTrajectoriesScores(critics_data_);
+    updateControlSequence();
+  }
+}
+
+bool Optimizer::fallback(bool fail)
+{
+  static size_t counter = 0;
+
+  if (!fail) {
+    counter = 0;
+    return false;
+  }
+
+  reset();
+
+  if (++counter > settings_.retry_attempt_limit) {
+    counter = 0;
+    throw std::runtime_error("Optimizer fail to compute path");
+  }
+
+  return true;
+}
+
+void Optimizer::prepare(
+  const geometry_msgs::msg::PoseStamped & robot_pose,
+  const geometry_msgs::msg::Twist & robot_speed,
+  const nav_msgs::msg::Path & plan,
+  const geometry_msgs::msg::Pose & goal,
+  nav2_core::GoalChecker * goal_checker)
+{
+  state_.pose = robot_pose;
+  state_.speed = robot_speed;
+  path_ = utils::toTensor(plan);
+  goal_ = goal;
+
+  costs_.fill(0);
+
+  critics_data_.fail_flag = false;
+  critics_data_.goal_checker = goal_checker;
+  critics_data_.motion_model = motion_model_;
+  critics_data_.furthest_reached_path_point.reset();
+  critics_data_.path_pts_valid.reset();
+}
+
+void Optimizer::shiftControlSequence()
+{
+  using namespace xt::placeholders;  // NOLINT
+  control_sequence_.vx = xt::roll(control_sequence_.vx, -1);
+  control_sequence_.wz = xt::roll(control_sequence_.wz, -1);
+
+
+  xt::view(control_sequence_.vx, -1) =
+    xt::view(control_sequence_.vx, -2);
+
+  xt::view(control_sequence_.wz, -1) =
+    xt::view(control_sequence_.wz, -2);
+
+
+  if (isHolonomic()) {
+    control_sequence_.vy = xt::roll(control_sequence_.vy, -1);
+    xt::view(control_sequence_.vy, -1) =
+      xt::view(control_sequence_.vy, -2);
+  }
+}
+
+void Optimizer::generateNoisedTrajectories()
+{
+  noise_generator_.setNoisedControls(state_, control_sequence_);
+  noise_generator_.generateNextNoises();
+  updateStateVelocities(state_);
+  integrateStateVelocities(generated_trajectories_, state_);
+}
+
+bool Optimizer::isHolonomic() const {return motion_model_->isHolonomic();}
+
+void Optimizer::applyControlSequenceConstraints()
+{
+  auto & s = settings_;
+
+  if (isHolonomic()) {
+    control_sequence_.vy = xt::clip(control_sequence_.vy, -s.constraints.vy, s.constraints.vy);
+  }
+
+  control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max);
+  control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz);
+
+  motion_model_->applyConstraints(control_sequence_);
+}
+
+void Optimizer::updateStateVelocities(
+  models::State & state) const
+{
+  updateInitialStateVelocities(state);
+  propagateStateVelocitiesFromInitials(state);
+}
+
+void Optimizer::updateInitialStateVelocities(
+  models::State & state) const
+{
+  xt::noalias(xt::view(state.vx, xt::all(), 0)) = state.speed.linear.x;
+  xt::noalias(xt::view(state.wz, xt::all(), 0)) = state.speed.angular.z;
+
+  if (isHolonomic()) {
+    xt::noalias(xt::view(state.vy, xt::all(), 0)) = state.speed.linear.y;
+  }
+}
+
+void Optimizer::propagateStateVelocitiesFromInitials(
+  models::State & state) const
+{
+  motion_model_->predict(state);
+}
+
+void Optimizer::integrateStateVelocities(
+  xt::xtensor<float, 2> & trajectory,
+  const xt::xtensor<float, 2> & sequence) const
+{
+  float initial_yaw = tf2::getYaw(state_.pose.pose.orientation);
+
+  const auto vx = xt::view(sequence, xt::all(), 0);
+  const auto vy = xt::view(sequence, xt::all(), 2);
+  const auto wz = xt::view(sequence, xt::all(), 1);
+
+  auto traj_x = xt::view(trajectory, xt::all(), 0);
+  auto traj_y = xt::view(trajectory, xt::all(), 1);
+  auto traj_yaws = xt::view(trajectory, xt::all(), 2);
+
+  xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw;
+
+  auto && yaw_cos = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
+  auto && yaw_sin = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
+
+  const auto yaw_offseted = xt::view(traj_yaws, xt::range(1, _));
+
+  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);
+
+  auto && dx = xt::eval(vx * yaw_cos);
+  auto && dy = xt::eval(vx * yaw_sin);
+
+  if (isHolonomic()) {
+    dx = dx - vy * yaw_sin;
+    dy = dy + vy * yaw_cos;
+  }
+
+  xt::noalias(traj_x) = state_.pose.pose.position.x + xt::cumsum(dx * settings_.model_dt, 0);
+  xt::noalias(traj_y) = state_.pose.pose.position.y + xt::cumsum(dy * settings_.model_dt, 0);
+}
+
+void Optimizer::integrateStateVelocities(
+  models::Trajectories & trajectories,
+  const models::State & state) const
+{
+  const float initial_yaw = tf2::getYaw(state.pose.pose.orientation);
+
+  xt::noalias(trajectories.yaws) =
+    xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw;
+
+  const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1));
+
+  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)) = 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);
+
+  auto && dx = xt::eval(state.vx * yaw_cos);
+  auto && dy = xt::eval(state.vx * yaw_sin);
+
+  if (isHolonomic()) {
+    dx = dx - state.vy * yaw_sin;
+    dy = dy + state.vy * yaw_cos;
+  }
+
+  xt::noalias(trajectories.x) = state.pose.pose.position.x +
+    xt::cumsum(dx * settings_.model_dt, 1);
+  xt::noalias(trajectories.y) = state.pose.pose.position.y +
+    xt::cumsum(dy * settings_.model_dt, 1);
+}
+
+xt::xtensor<float, 2> Optimizer::getOptimizedTrajectory()
+{
+  auto && sequence =
+    xt::xtensor<float, 2>::from_shape({settings_.time_steps, isHolonomic() ? 3u : 2u});
+  auto && trajectories = xt::xtensor<float, 2>::from_shape({settings_.time_steps, 3});
+
+  xt::noalias(xt::view(sequence, xt::all(), 0)) = control_sequence_.vx;
+  xt::noalias(xt::view(sequence, xt::all(), 1)) = control_sequence_.wz;
+
+  if (isHolonomic()) {
+    xt::noalias(xt::view(sequence, xt::all(), 2)) = control_sequence_.vy;
+  }
+
+  integrateStateVelocities(trajectories, sequence);
+  return std::move(trajectories);
+}
+
+void Optimizer::updateControlSequence()
+{
+  auto & s = settings_;
+  auto bounded_noises_vx = state_.cvx - control_sequence_.vx;
+  auto bounded_noises_wz = state_.cwz - control_sequence_.wz;
+  xt::noalias(costs_) +=
+    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 / 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 / powf(s.sampling_std.vy, 2) * xt::sum(
+      xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy,
+      1, immediate);
+  }
+
+  auto && costs_normalized = costs_ - xt::amin(costs_, immediate);
+  auto && exponents = xt::eval(xt::exp(-1 / settings_.temperature * costs_normalized));
+  auto && softmaxes = xt::eval(exponents / xt::sum(exponents, immediate));
+  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_.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();
+}
+
+geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  unsigned int offset = settings_.shift_control_sequence ? 1 : 0;
+
+  auto vx = control_sequence_.vx(offset);
+  auto wz = control_sequence_.wz(offset);
+
+  if (isHolonomic()) {
+    auto vy = control_sequence_.vy(offset);
+    return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());
+  }
+
+  return utils::toTwistStamped(vx, wz, stamp, costmap_ros_->getBaseFrameID());
+}
+
+void Optimizer::setMotionModel(const std::string & model)
+{
+  if (model == "DiffDrive") {
+    motion_model_ = std::make_shared<DiffDriveMotionModel>();
+  } else if (model == "Omni") {
+    motion_model_ = std::make_shared<OmniMotionModel>();
+  } else if (model == "Ackermann") {
+    motion_model_ = std::make_shared<AckermannMotionModel>(parameters_handler_, name_);
+  } else {
+    throw std::runtime_error(
+            std::string(
+              "Model " + model + " is not valid! Valid options are DiffDrive, Omni, "
+              "or Ackermann"));
+  }
+}
+
+void Optimizer::setSpeedLimit(double speed_limit, bool percentage)
+{
+  auto & s = settings_;
+  if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
+    s.constraints.vx_max = s.base_constraints.vx_max;
+    s.constraints.vx_min = s.base_constraints.vx_min;
+    s.constraints.vy = s.base_constraints.vy;
+    s.constraints.wz = s.base_constraints.wz;
+  } else {
+    if (percentage) {
+      // Speed limit is expressed in % from maximum speed of robot
+      double ratio = speed_limit / 100.0;
+      s.constraints.vx_max = s.base_constraints.vx_max * ratio;
+      s.constraints.vx_min = s.base_constraints.vx_min * ratio;
+      s.constraints.vy = s.base_constraints.vy * ratio;
+      s.constraints.wz = s.base_constraints.wz * ratio;
+    } else {
+      // Speed limit is expressed in absolute value
+      double ratio = speed_limit / s.base_constraints.vx_max;
+      s.constraints.vx_max = s.base_constraints.vx_max * ratio;
+      s.constraints.vx_min = s.base_constraints.vx_min * ratio;
+      s.constraints.vy = s.base_constraints.vy * ratio;
+      s.constraints.wz = s.base_constraints.wz * ratio;
+    }
+  }
+}
+
+models::Trajectories & Optimizer::getGeneratedTrajectories()
+{
+  return generated_trajectories_;
+}
+
+}  // namespace mppi
@@ -0,0 +1,72 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/parameters_handler.hpp"
+
+namespace mppi
+{
+
+ParametersHandler::ParametersHandler(
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent)
+{
+  node_ = parent;
+  auto node = node_.lock();
+  node_name_ = node->get_name();
+  logger_ = node->get_logger();
+}
+
+void ParametersHandler::start()
+{
+  auto node = node_.lock();
+  on_set_param_handler_ = node->add_on_set_parameters_callback(
+    std::bind(
+      &ParametersHandler::dynamicParamsCallback, this,
+      std::placeholders::_1));
+
+  auto get_param = getParamGetter(node_name_);
+  get_param(verbose_, "verbose", false);
+}
+
+rcl_interfaces::msg::SetParametersResult
+ParametersHandler::dynamicParamsCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock(parameters_change_mutex_);
+
+  for (auto & pre_cb : pre_callbacks_) {
+    pre_cb();
+  }
+
+  for (auto & param : parameters) {
+    const std::string & param_name = param.get_name();
+
+    if (auto callback = get_param_callbacks_.find(param_name);
+      callback != get_param_callbacks_.end())
+    {
+      callback->second(param);
+    } else {
+      RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str());
+    }
+  }
+
+  for (auto & post_cb : post_callbacks_) {
+    post_cb();
+  }
+
+  result.successful = true;
+  return result;
+}
+
+}  // namespace mppi
@@ -0,0 +1,220 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2023 Dexory
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_mppi_controller/tools/path_handler.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_costmap_2d/costmap_2d_ros.hpp"
+
+namespace mppi
+{
+
+void PathHandler::initialize(
+  rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
+  std::shared_ptr<tf2_ros::Buffer> buffer, ParametersHandler * param_handler)
+{
+  name_ = name;
+  costmap_ = costmap;
+  tf_buffer_ = buffer;
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  parameters_handler_ = param_handler;
+
+  auto getParam = parameters_handler_->getParamGetter(name_);
+  getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
+  getParam(prune_distance_, "prune_distance", 1.5);
+  getParam(transform_tolerance_, "transform_tolerance", 0.1);
+  getParam(enforce_path_inversion_, "enforce_path_inversion", false);
+  if (enforce_path_inversion_) {
+    getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
+    getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4);
+    inversion_locale_ = 0u;
+  }
+}
+
+std::pair<nav_msgs::msg::Path, PathIterator>
+PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
+  const geometry_msgs::msg::PoseStamped & global_pose)
+{
+  using nav2_util::geometry_utils::euclidean_distance;
+
+  auto begin = global_plan_up_to_inversion_.poses.begin();
+
+  // Limit the search for the closest pose up to max_robot_pose_search_dist on the path
+  auto closest_pose_upper_bound =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
+    max_robot_pose_search_dist_);
+
+  // Find closest point to the robot
+  auto closest_point = nav2_util::geometry_utils::min_by(
+    begin, closest_pose_upper_bound,
+    [&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
+      return euclidean_distance(global_pose, ps);
+    });
+
+  nav_msgs::msg::Path transformed_plan;
+  transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
+  transformed_plan.header.stamp = global_pose.header.stamp;
+
+  auto pruned_plan_end =
+    nav2_util::geometry_utils::first_after_integrated_distance(
+    closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
+
+  unsigned int mx, my;
+  // Find the furthest relevent pose on the path to consider within costmap
+  // bounds
+  // Transforming it to the costmap frame in the same loop
+  for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
+    ++global_plan_pose)
+  {
+    // Transform from global plan frame to costmap frame
+    geometry_msgs::msg::PoseStamped costmap_plan_pose;
+    global_plan_pose->header.stamp = global_pose.header.stamp;
+    global_plan_pose->header.frame_id = global_plan_.header.frame_id;
+    transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
+
+    // Check if pose is inside the costmap
+    if (!costmap_->getCostmap()->worldToMap(
+        costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
+    {
+      return {transformed_plan, closest_point};
+    }
+
+    // Filling the transformed plan to return with the transformed pose
+    transformed_plan.poses.push_back(costmap_plan_pose);
+  }
+
+  return {transformed_plan, closest_point};
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
+  const geometry_msgs::msg::PoseStamped & pose)
+{
+  if (global_plan_up_to_inversion_.poses.empty()) {
+    throw std::runtime_error("Received plan with zero length");
+  }
+
+  geometry_msgs::msg::PoseStamped robot_pose;
+  if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) {
+    throw std::runtime_error(
+            "Unable to transform robot pose into global plan's frame");
+  }
+
+  return robot_pose;
+}
+
+nav_msgs::msg::Path PathHandler::transformPath(
+  const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Find relevent bounds of path to use
+  geometry_msgs::msg::PoseStamped global_pose =
+    transformToGlobalPlanFrame(robot_pose);
+  auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
+
+  prunePlan(global_plan_up_to_inversion_, lower_bound);
+
+  if (enforce_path_inversion_ && inversion_locale_ != 0u) {
+    if (isWithinInversionTolerances(global_pose)) {
+      prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
+      global_plan_up_to_inversion_ = global_plan_;
+      inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+    }
+  }
+
+  if (transformed_plan.poses.empty()) {
+    throw std::runtime_error("Resulting plan has 0 poses in it.");
+  }
+
+  return transformed_plan;
+}
+
+bool PathHandler::transformPose(
+  const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
+  geometry_msgs::msg::PoseStamped & out_pose) const
+{
+  if (in_pose.header.frame_id == frame) {
+    out_pose = in_pose;
+    return true;
+  }
+
+  try {
+    tf_buffer_->transform(
+      in_pose, out_pose, frame,
+      tf2::durationFromSec(transform_tolerance_));
+    out_pose.header.frame_id = frame;
+    return true;
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what());
+  }
+  return false;
+}
+
+double PathHandler::getMaxCostmapDist()
+{
+  const auto & costmap = costmap_->getCostmap();
+  return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
+         costmap->getResolution() * 0.50;
+}
+
+void PathHandler::setPath(const nav_msgs::msg::Path & plan)
+{
+  global_plan_ = plan;
+  global_plan_up_to_inversion_ = global_plan_;
+  if (enforce_path_inversion_) {
+    inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
+  }
+}
+
+nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
+
+void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
+{
+  plan.poses.erase(plan.poses.begin(), end);
+}
+
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
+  const builtin_interfaces::msg::Time & stamp)
+{
+  auto goal = global_plan_.poses.back();
+  goal.header.frame_id = global_plan_.header.frame_id;
+  goal.header.stamp = stamp;
+  if (goal.header.frame_id.empty()) {
+    throw std::runtime_error("Goal pose has an empty frame_id");
+  }
+  geometry_msgs::msg::PoseStamped transformed_goal;
+  if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) {
+    throw std::runtime_error("Unable to transform goal pose into costmap frame");
+  }
+  return transformed_goal;
+}
+
+bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
+{
+  // Keep full path if we are within tolerance of the inversion pose
+  const auto last_pose = global_plan_up_to_inversion_.poses.back();
+  float distance = hypotf(
+    robot_pose.pose.position.x - last_pose.pose.position.x,
+    robot_pose.pose.position.y - last_pose.pose.position.y);
+
+  float angle_distance = angles::shortest_angular_distance(
+    tf2::getYaw(robot_pose.pose.orientation),
+    tf2::getYaw(last_pose.pose.orientation));
+
+  return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
+}
+
+}  // namespace mppi
@@ -0,0 +1,130 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
+
+namespace mppi
+{
+
+void TrajectoryVisualizer::on_configure(
+  rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
+  const std::string & frame_id, ParametersHandler * parameters_handler)
+{
+  auto node = parent.lock();
+  logger_ = node->get_logger();
+  frame_id_ = frame_id;
+  trajectories_publisher_ =
+    node->create_publisher<visualization_msgs::msg::MarkerArray>("/trajectories", 1);
+  transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
+  parameters_handler_ = parameters_handler;
+
+  auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
+
+  getParam(trajectory_step_, "trajectory_step", 5);
+  getParam(time_step_, "time_step", 3);
+
+  reset();
+}
+
+void TrajectoryVisualizer::on_cleanup()
+{
+  trajectories_publisher_.reset();
+  transformed_path_pub_.reset();
+}
+
+void TrajectoryVisualizer::on_activate()
+{
+  trajectories_publisher_->on_activate();
+  transformed_path_pub_->on_activate();
+}
+
+void TrajectoryVisualizer::on_deactivate()
+{
+  trajectories_publisher_->on_deactivate();
+  transformed_path_pub_->on_deactivate();
+}
+
+void TrajectoryVisualizer::add(
+  const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace)
+{
+  auto & size = trajectory.shape()[0];
+  if (!size) {
+    return;
+  }
+
+  auto add_marker = [&](auto i) {
+      float component = static_cast<float>(i) / static_cast<float>(size);
+
+      auto pose = utils::createPose(trajectory(i, 0), trajectory(i, 1), 0.06);
+      auto scale =
+        i != size - 1 ?
+        utils::createScale(0.03, 0.03, 0.07) :
+        utils::createScale(0.07, 0.07, 0.09);
+      auto color = utils::createColor(0, component, component, 1);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
+      points_->markers.push_back(marker);
+    };
+
+  for (size_t i = 0; i < size; i++) {
+    add_marker(i);
+  }
+}
+
+void TrajectoryVisualizer::add(
+  const models::Trajectories & trajectories, const std::string & marker_namespace)
+{
+  auto & shape = trajectories.x.shape();
+  const float shape_1 = static_cast<float>(shape[1]);
+  points_->markers.reserve(floor(shape[0] / trajectory_step_) * floor(shape[1] * time_step_));
+
+  for (size_t i = 0; i < shape[0]; i += trajectory_step_) {
+    for (size_t j = 0; j < shape[1]; j += time_step_) {
+      const float j_flt = static_cast<float>(j);
+      float blue_component = 1.0f - j_flt / shape_1;
+      float green_component = j_flt / shape_1;
+
+      auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03);
+      auto scale = utils::createScale(0.03, 0.03, 0.03);
+      auto color = utils::createColor(0, green_component, blue_component, 1);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
+
+      points_->markers.push_back(marker);
+    }
+  }
+}
+
+void TrajectoryVisualizer::reset()
+{
+  marker_id_ = 0;
+  points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
+}
+
+void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
+{
+  if (trajectories_publisher_->get_subscription_count() > 0) {
+    trajectories_publisher_->publish(std::move(points_));
+  }
+
+  reset();
+
+  if (transformed_path_pub_->get_subscription_count() > 0) {
+    auto plan_ptr = std::make_unique<nav_msgs::msg::Path>(plan);
+    transformed_path_pub_->publish(std::move(plan_ptr));
+  }
+}
+
+}  // namespace mppi