Publishing a full time-series MPPI trajectory if desirable

nav2_bringup/params/nav2_params.yaml

@@ -131,6 +131,7 @@ controller_server:
       gamma: 0.015
       motion_model: "DiffDrive"
       visualize: true
+      publish_optimal_trajectory: true
       regenerate_noises: true
       TrajectoryVisualizer:
         trajectory_step: 5

nav2_mppi_controller/README.md

@@ -58,6 +58,8 @@ This process is then repeated a number of times and returns a converged solution
  | visualize                  | bool   | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging.                                                                                                                                       |
  | retry_attempt_limit        | int    | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure.                                                                                                                                                                                                       |
  | regenerate_noises          | bool   | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. |
+ | publish_optimal_trajectory | bool   | Publishes the full optimal trajectory sequence each control iteration for downstream  control systems, collision checkers, etc to have context beyond the next timestep. |
+
 
 #### Trajectory Visualizer
  | Parameter             | Type   | Definition                                                                                                  |

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -105,23 +105,28 @@ class MPPIController : public nav2_core::Controller
   /**
     * @brief Visualize trajectories
     * @param transformed_plan Transformed input plan
+    * @param cmd_stamp Command stamp
+    * @param optimal_trajectory Optimal trajectory, if already computed
     */
   void visualize(
     nav_msgs::msg::Path transformed_plan,
-    const builtin_interfaces::msg::Time & cmd_stamp);
+    const builtin_interfaces::msg::Time & cmd_stamp,
+    const Eigen::ArrayXXf & optimal_trajectory);
 
   std::string name_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Trajectory>::SharedPtr opt_traj_pub_;
 
   std::unique_ptr<ParametersHandler> parameters_handler_;
   Optimizer optimizer_;
   PathHandler path_handler_;
   TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
+  bool publish_optimal_trajectory_;
 };
 
 }  // namespace nav2_mppi_controller

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2025 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.

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -105,6 +105,12 @@ class Optimizer
    */
   Eigen::ArrayXXf getOptimizedTrajectory();
 
+  /**
+   * @brief Get the optimal control sequence for a cycle for visualization
+   * @return Optimal control sequence
+   */
+  const models::ControlSequence & getOptimalControlSequence();
+
   /**
    * @brief Set the maximum speed based on the speed limits callback
    * @param speed_limit Limit of the speed for use
@@ -117,6 +123,15 @@ class Optimizer
    */
   void reset();
 
+  /**
+   * @brief Get the motion model time step
+   * @return Time step of the model
+   */
+  const models::OptimizerSettings & getSettings() const
+  {
+    return settings_;
+  }
+
 protected:
   /**
    * @brief Main function to generate, score, and return trajectories

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -32,6 +32,7 @@
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav_msgs/msg/path.hpp"
+#include "nav2_msgs/msg/trajectory.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
 #include "rclcpp/rclcpp.hpp"
@@ -170,6 +171,34 @@ inline geometry_msgs::msg::TwistStamped toTwistStamped(
   return twist;
 }
 
+inline std::unique_ptr<nav2_msgs::msg::Trajectory> toTrajectoryMsg(
+  const Eigen::ArrayXXf & trajectory,
+  const models::ControlSequence & control_sequence,
+  const double & model_dt,
+  const std_msgs::msg::Header & header)
+{
+  auto trajectory_msg = std::make_unique<nav2_msgs::msg::Trajectory>();
+  trajectory_msg->header = header;
+  trajectory_msg->points.resize(trajectory.rows());
+
+  for (int i = 0; i < trajectory.rows(); ++i) {
+    auto & curr_pt = trajectory_msg->points[i];
+    curr_pt.time_from_start = rclcpp::Duration::from_seconds(i * model_dt);
+    curr_pt.pose.position.x = trajectory(i, 0);
+    curr_pt.pose.position.y = trajectory(i, 1);
+    tf2::Quaternion quat;
+    quat.setRPY(0.0, 0.0, trajectory(i, 2));
+    curr_pt.pose.orientation = tf2::toMsg(quat);
+    curr_pt.velocity.linear.x = control_sequence.vx(i);
+    curr_pt.velocity.angular.z = control_sequence.wz(i);
+    if (control_sequence.vy.size() > 0) {
+      curr_pt.velocity.linear.y = control_sequence.vy(i);
+    }
+  }
+
+  return trajectory_msg;
+}
+
 /**
  * @brief Convert path to a tensor
  * @param path Path to convert

nav2_mppi_controller/src/controller.cpp

@@ -38,13 +38,20 @@
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(visualize_, "visualize", false);
 
+  getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false);
+
   // 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());
 
+  if (publish_optimal_trajectory_) {
+    opt_traj_pub_ = node->create_publisher<nav2_msgs::msg::Trajectory>(
+      "~/optimal_trajectory", rclcpp::SystemDefaultsQoS());
+  }
+
   RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
 }
 
@@ -53,19 +60,27 @@
   optimizer_.shutdown();
   trajectory_visualizer_.on_cleanup();
   parameters_handler_.reset();
+  opt_traj_pub_.reset();
   RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::activate()
 {
+  auto node = parent_.lock();
   trajectory_visualizer_.on_activate();
   parameters_handler_->start();
+  if (opt_traj_pub_) {
+    opt_traj_pub_->on_activate();
+  }
   RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::deactivate()
 {
   trajectory_visualizer_.on_deactivate();
+  if (opt_traj_pub_) {
+    opt_traj_pub_->on_deactivate();
+  }
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
 
@@ -100,19 +115,36 @@
   RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
 #endif
 
+  Eigen::ArrayXXf optimal_trajectory;
+  if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) {
+    optimal_trajectory = optimizer_.getOptimizedTrajectory();
+    auto trajectory_msg = utils::toTrajectoryMsg(
+      optimal_trajectory,
+      optimizer_.getOptimalControlSequence(),
+      optimizer_.getSettings().model_dt,
+      cmd.header);
+    opt_traj_pub_->publish(std::move(trajectory_msg));
+  }
+
   if (visualize_) {
-    visualize(std::move(transformed_plan), cmd.header.stamp);
+    visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory);
   }
 
   return cmd;
 }
 
 void MPPIController::visualize(
   nav_msgs::msg::Path transformed_plan,
-  const builtin_interfaces::msg::Time & cmd_stamp)
+  const builtin_interfaces::msg::Time & cmd_stamp,
+  const Eigen::ArrayXXf & optimal_trajectory)
 {
   trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
+  if (optimal_trajectory.size() > 0) {
+    trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  } else {
+    trajectory_visualizer_.add(
+      optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
+  }
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 

nav2_mppi_controller/src/optimizer.cpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+// Copyright (c) 2025 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.
@@ -426,6 +427,11 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory()
   return trajectories;
 }
 
+const models::ControlSequence & Optimizer::getOptimalControlSequence()
+{
+  return control_sequence_;
+}
+
 void Optimizer::updateControlSequence()
 {
   const bool is_holo = isHolonomic();

nav2_mppi_controller/src/trajectory_visualizer.cpp

@@ -26,9 +26,10 @@ void TrajectoryVisualizer::on_configure(
   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);
-  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("optimal_trajectory", 1);
+    node->create_publisher<visualization_msgs::msg::MarkerArray>("~/candidate_trajectories", 1);
+  transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>(
+    "~/transformed_global_plan", 1);
+  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/optimal_path", 1);
   parameters_handler_ = parameters_handler;
 
   auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");

nav2_mppi_controller/test/controller_state_transition_test.cpp

@@ -38,6 +38,7 @@ TEST(ControllerStateTransitionTest, ControllerNotFail)
   options.parameter_overrides(params);
 
   auto node = getDummyNode(options);
+  node->declare_parameter("publish_optimal_trajectory", true);
   auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   auto costmap_ros = getDummyCostmapRos(costmap_settings);
   costmap_ros->setRobotFootprint(getDummySquareFootprint(0.01));

nav2_mppi_controller/test/optimizer_unit_tests.cpp

@@ -650,6 +650,26 @@ TEST(OptimizerTests, integrateStateVelocitiesTests)
   }
 }
 
+TEST(OptimizerTests, TestGetters)
+{
+  OptimizerTester optimizer_tester;
+  optimizer_tester.fillOptimizerWithGarbage();
+
+  auto control_seq = optimizer_tester.getOptimalControlSequence();
+  EXPECT_EQ(control_seq.vx(0), 342.0);
+  EXPECT_EQ(control_seq.vx.rows(), 30);
+
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+  optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
+  EXPECT_EQ(optimizer_tester.getSettings().model_dt, 0.05f);
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp

@@ -45,7 +45,7 @@ TEST(TrajectoryVisualizerTests, VisPathRepub)
   pub_path.poses.resize(5);
 
   auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
-    "transformed_global_plan", 10,
+    "~/transformed_global_plan", 10,
     [&](const nav_msgs::msg::Path msg) {received_path = msg;});
 
   TrajectoryVisualizer vis;
@@ -65,7 +65,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
 
   visualization_msgs::msg::MarkerArray received_msg;
   auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
-    "trajectories", 10,
+    "~/candidate_trajectories", 10,
     [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
 
   // optimal_trajectory empty, should fail to publish
@@ -127,7 +127,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
 
   visualization_msgs::msg::MarkerArray received_msg;
   auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
-    "trajectories", 10,
+    "~/candidate_trajectories", 10,
     [&](const visualization_msgs::msg::MarkerArray msg) {received_msg = msg;});
 
   models::Trajectories candidate_trajectories;
@@ -157,7 +157,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
 
   nav_msgs::msg::Path received_path;
   auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
-    "optimal_trajectory", 10,
+    "~/optimal_path", 10,
     [&](const nav_msgs::msg::Path msg) {received_path = msg;});
 
   // optimal_trajectory empty, should fail to publish

nav2_mppi_controller/test/utils_test.cpp

@@ -569,6 +569,65 @@ TEST(UtilsTests, NormalizeYawsBetweenPointsTest)
   EXPECT_NEAR(yaws_between_points_corrected[1], -0.8 * M_PIF_2, 1e-3);
 }
 
+TEST(UtilsTests, toTrajectoryMsgTest)
+{
+  Eigen::ArrayXXf trajectory(5, 3);
+  trajectory <<
+    0.0, 0.0, 0.0,
+    1.0, 1.0, 1.0,
+    2.0, 2.0, 2.0,
+    3.0, 3.0, 3.0,
+    4.0, 4.0, 4.0;
+
+  models::ControlSequence control_sequence;
+  control_sequence.vx = Eigen::ArrayXf::Ones(5);
+  control_sequence.wz = Eigen::ArrayXf::Ones(5);
+  control_sequence.vy = Eigen::ArrayXf::Zero(5);
+
+  std_msgs::msg::Header header;
+  header.frame_id = "map";
+  header.stamp = rclcpp::Time(100, 0, RCL_ROS_TIME);
+
+  auto trajectory_msg = utils::toTrajectoryMsg(
+    trajectory, control_sequence, 1.0, header);
+
+  EXPECT_EQ(trajectory_msg->header.frame_id, "map");
+  EXPECT_EQ(trajectory_msg->header.stamp, header.stamp);
+  EXPECT_EQ(trajectory_msg->points.size(), 5u);
+  EXPECT_EQ(trajectory_msg->points[0].pose.position.x, 0.0);
+  EXPECT_EQ(trajectory_msg->points[0].pose.position.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[1].pose.position.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].pose.position.y, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].pose.position.x, 2.0);
+  EXPECT_EQ(trajectory_msg->points[2].pose.position.y, 2.0);
+  EXPECT_EQ(trajectory_msg->points[3].pose.position.x, 3.0);
+  EXPECT_EQ(trajectory_msg->points[3].pose.position.y, 3.0);
+  EXPECT_EQ(trajectory_msg->points[4].pose.position.x, 4.0);
+  EXPECT_EQ(trajectory_msg->points[4].pose.position.y, 4.0);
+
+  EXPECT_EQ(trajectory_msg->points[0].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[0].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[0].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[1].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[2].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[3].velocity.angular.z, 1.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.linear.x, 1.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.linear.y, 0.0);
+  EXPECT_EQ(trajectory_msg->points[4].velocity.angular.z, 1.0);
+
+  EXPECT_EQ(trajectory_msg->points[0].time_from_start, rclcpp::Duration(0, 0));
+  EXPECT_EQ(trajectory_msg->points[1].time_from_start, rclcpp::Duration(1, 0));
+  EXPECT_EQ(trajectory_msg->points[2].time_from_start, rclcpp::Duration(2, 0));
+  EXPECT_EQ(trajectory_msg->points[3].time_from_start, rclcpp::Duration(3, 0));
+  EXPECT_EQ(trajectory_msg->points[4].time_from_start, rclcpp::Duration(4, 0));
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

nav2_msgs/CMakeLists.txt

@@ -32,6 +32,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Particle.msg"
   "msg/ParticleCloud.msg"
   "msg/MissedWaypoint.msg"
+  "msg/Trajectory.msg"
+  "msg/TrajectoryPoint.msg"
   "srv/GetCosts.srv"
   "srv/GetCostmap.srv"
   "srv/IsPathValid.srv"

nav2_msgs/msg/Trajectory.msg

@@ -0,0 +1,7 @@
+# An array of trajectory points that represents a trajectory for a robot to follow.
+
+# Indicates the frame_id of the trajectory.
+std_msgs/Header header
+
+# Array of trajectory points to follow.
+TrajectoryPoint[] points