diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
index ae041c1d32..328424317d 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp
@@ -89,14 +89,6 @@ class TrajectoryVisualizer
     */
   void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
 
-  /**
-    * @brief Publish the optimal trajectory in the form of a path message
-    * @param trajectory Optimal trajectory
-    */
-  void populate_optimal_path(
-    const xt::xtensor<float, 2> & optimal_traj,
-    const builtin_interfaces::msg::Time & cmd_stamp);
-
   /**
     * @brief Visualize the plan
     * @param plan Plan to visualize
diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp
index ed2d3e89b1..7cb07c6858 100644
--- a/nav2_mppi_controller/src/trajectory_visualizer.cpp
+++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp
@@ -82,13 +82,24 @@ void TrajectoryVisualizer::add(
       auto marker = utils::createMarker(
         marker_id_++, pose, scale, color, frame_id_, marker_namespace);
       points_->markers.push_back(marker);
+
+      // populate optimal path
+      geometry_msgs::msg::PoseStamped pose_stamped;
+      pose_stamped.header.frame_id = frame_id_;
+      pose_stamped.pose = pose;
+
+      tf2::Quaternion quaternion_tf2;
+      quaternion_tf2.setRPY(0., 0., trajectory(i, 2));
+      pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
+
+      optimal_path_->poses.push_back(pose_stamped);
     };
 
+  optimal_path_->header.stamp = cmd_stamp;
+  optimal_path_->header.frame_id = frame_id_;
   for (size_t i = 0; i < size; i++) {
     add_marker(i);
   }
-
-  populate_optimal_path(trajectory, cmd_stamp);
 }
 
 void TrajectoryVisualizer::add(
@@ -140,33 +151,4 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
   }
 }
 
-void TrajectoryVisualizer::populate_optimal_path(
-  const xt::xtensor<float, 2> & optimal_traj,
-  const builtin_interfaces::msg::Time & cmd_stamp)
-{
-  auto & size = optimal_traj.shape()[0];
-  if (size == 0) {
-    return;
-  }
-
-  optimal_path_->header.stamp = cmd_stamp;
-  optimal_path_->header.frame_id = frame_id_;
-
-  for (size_t i = 0; i < size; i++) {
-    // create new pose for the path
-    geometry_msgs::msg::PoseStamped pose_stamped;
-    pose_stamped.header.frame_id = frame_id_;
-
-    // position & orientation
-    pose_stamped.pose = utils::createPose(optimal_traj(i, 0), optimal_traj(i, 1), 0.0);
-
-    tf2::Quaternion quaternion_tf2;
-    quaternion_tf2.setRPY(0., 0., optimal_traj(i, 2));
-    pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
-
-    // add pose to the path
-    optimal_path_->poses.push_back(pose_stamped);
-  }
-}
-
 }  // namespace mppi
diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
index 8b28eeb7ff..2b7c8e0a90 100644
--- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
+++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
@@ -205,7 +205,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
     // Check positions are correct
     EXPECT_EQ(recieved_path.poses[i].pose.position.x, static_cast<float>(i));
     EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast<float>(i));
-    EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.0);
+    EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.06);
 
     // Check orientations are correct
     quat.setRPY(0., 0., optimal_trajectory(i, 2));