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 ba4dde4ca27..6ecdbdcb8dc 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
@@ -73,13 +73,13 @@ class TrajectoryVisualizer
     * @brief Add an optimal trajectory to visualize
     * @param trajectory Optimal trajectory
     */
-  void add(const xt::xtensor<float, 2> & trajectory);
+  void add(const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace);
 
   /**
     * @brief Add candidate trajectories to visualize
     * @param trajectories Candidate trajectories
     */
-  void add(const models::Trajectories & trajectories);
+  void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
 
   /**
     * @brief Visualize the plan
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
index 2577e82488b..06a042c7246 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp
@@ -118,13 +118,13 @@ inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
  */
 inline visualization_msgs::msg::Marker createMarker(
   int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
-  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id)
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
 {
   using visualization_msgs::msg::Marker;
   Marker marker;
   marker.header.frame_id = frame_id;
   marker.header.stamp = rclcpp::Time(0, 0);
-  marker.ns = "MarkerNS";
+  marker.ns = ns;
   marker.id = id;
   marker.type = Marker::SPHERE;
   marker.action = Marker::ADD;
@@ -371,7 +371,7 @@ inline void findPathCosts(
   auto * costmap = costmap_ros->getCostmap();
   unsigned int map_x, map_y;
   const size_t path_segments_count = data.path.x.shape(0) - 1;
-  data.path_pts_valid = std::vector<bool>(path_segments_count - 1, false);
+  data.path_pts_valid = std::vector<bool>(path_segments_count, false);
   for (unsigned int idx = 0; idx < path_segments_count; idx++) {
     const auto path_x = data.path.x(idx);
     const auto path_y = data.path.y(idx);
diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
index 0cf5cf2259f..5f8dcffd261 100644
--- a/nav2_mppi_controller/src/controller.cpp
+++ b/nav2_mppi_controller/src/controller.cpp
@@ -104,8 +104,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
 
 void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
 {
-  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories());
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory());
+  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 
diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp
index 7eaaca60998..8b951114e7d 100644
--- a/nav2_mppi_controller/src/path_handler.cpp
+++ b/nav2_mppi_controller/src/path_handler.cpp
@@ -81,6 +81,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
     // 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
diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp
index 320a5856d38..a6531b7d1df 100644
--- a/nav2_mppi_controller/src/trajectory_visualizer.cpp
+++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp
@@ -56,7 +56,8 @@ void TrajectoryVisualizer::on_deactivate()
   transformed_path_pub_->on_deactivate();
 }
 
-void TrajectoryVisualizer::add(const xt::xtensor<float, 2> & trajectory)
+void TrajectoryVisualizer::add(
+  const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace)
 {
   auto & size = trajectory.shape()[0];
   if (!size) {
@@ -72,7 +73,8 @@ void TrajectoryVisualizer::add(const xt::xtensor<float, 2> & trajectory)
         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_);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
       points_->markers.push_back(marker);
     };
 
@@ -82,7 +84,7 @@ void TrajectoryVisualizer::add(const xt::xtensor<float, 2> & trajectory)
 }
 
 void TrajectoryVisualizer::add(
-  const models::Trajectories & trajectories)
+  const models::Trajectories & trajectories, const std::string & marker_namespace)
 {
   auto & shape = trajectories.x.shape();
   const float shape_1 = static_cast<float>(shape[1]);
@@ -97,7 +99,8 @@ void TrajectoryVisualizer::add(
       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_);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
 
       points_->markers.push_back(marker);
     }
diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
index 7417856caee..7ebada2a6a2 100644
--- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
+++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp
@@ -81,7 +81,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(optimal_trajectory);
+  vis.add(optimal_trajectory, "Optimal Trajectory");
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
@@ -90,7 +90,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
 
   // Now populated with content, should publish
   optimal_trajectory = xt::ones<float>({20, 2});
-  vis.add(optimal_trajectory);
+  vis.add(optimal_trajectory, "Optimal Trajectory");
   vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
@@ -145,7 +145,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(candidate_trajectories);
+  vis.add(candidate_trajectories, "Candidate Trajectories");
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp
index bd5a0104485..ef4f1ee2374 100644
--- a/nav2_mppi_controller/test/utils_test.cpp
+++ b/nav2_mppi_controller/test/utils_test.cpp
@@ -80,12 +80,13 @@ TEST(UtilsTests, MarkerPopulationUtils)
   EXPECT_EQ(color.b, 3.0);
   EXPECT_EQ(color.a, 0.0);
 
-  auto marker = createMarker(999, pose, scale, color, "map");
+  auto marker = createMarker(999, pose, scale, color, "map", "ns");
   EXPECT_EQ(marker.header.frame_id, "map");
   EXPECT_EQ(marker.id, 999);
   EXPECT_EQ(marker.pose, pose);
   EXPECT_EQ(marker.scale, scale);
   EXPECT_EQ(marker.color, color);
+  EXPECT_EQ(marker.ns, "ns");
 }
 
 TEST(UtilsTests, ConversionTests)