Publishing a full time-series MPPI trajectory if desirable (ros-navigation#5057)

* Publishing a full MPPI trajectory if desirable

Signed-off-by: Steve Macenski <[email protected]>

* only create publisher when needed

Signed-off-by: Steve Macenski <[email protected]>

* adjust topics

Signed-off-by: Steve Macenski <[email protected]>

* Moving for lint

Signed-off-by: Steve Macenski <[email protected]>

* updating topics in tests

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: stevedanomodolor <[email protected]>

Author: SteveMacenski

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 @@ void MPPIController::configure(
   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 @@ void MPPIController::cleanup()
   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 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   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));