From 1c56c7a532a8df9783c742961e341542e1e609e6 Mon Sep 17 00:00:00 2001
From: Alyssa Agnissan <alyssa.agnissan@quantillion.io>
Date: Thu, 22 Aug 2024 11:55:10 +0200
Subject: [PATCH] Publish optimal trajectory as a Path message

Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io>
---
 .../nav2_mppi_controller/controller.hpp       |  8 ++++
 nav2_mppi_controller/src/controller.cpp       | 38 +++++++++++++++++++
 2 files changed, 46 insertions(+)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
index 82b850bc460..a5471b56690 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp
@@ -108,6 +108,12 @@ class MPPIController : public nav2_core::Controller
     */
   void visualize(nav_msgs::msg::Path transformed_plan);
 
+   /**
+    * @brief Publish the optimal trajectory in the form of a path message
+    * @param trajectory Optimal trajectory
+    */
+  void publish_optimal_path(const xt::xtensor<float, 2> & optimal_traj);
+
   std::string name_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
@@ -119,6 +125,8 @@ class MPPIController : public nav2_core::Controller
   PathHandler path_handler_;
   TrajectoryVisualizer trajectory_visualizer_;
 
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr optimal_path_pub_;
+
   bool visualize_;
 };
 
diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp
index 6c831312633..594bba7ce37 100644
--- a/nav2_mppi_controller/src/controller.cpp
+++ b/nav2_mppi_controller/src/controller.cpp
@@ -45,6 +45,8 @@ void MPPIController::configure(
     parent_, name_,
     costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
 
+  optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
+
   RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
 }
 
@@ -60,12 +62,14 @@ void MPPIController::activate()
 {
   trajectory_visualizer_.on_activate();
   parameters_handler_->start();
+  optimal_path_pub_->on_activate();
   RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::deactivate()
 {
   trajectory_visualizer_.on_deactivate();
+  optimal_path_pub_->on_deactivate();
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
 
@@ -98,6 +102,10 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
 #endif
 
+  if (optimal_path_pub_->get_subscription_count() > 0) {
+    publish_optimal_path(optimizer_.getOptimizedTrajectory());
+  }
+
   if (visualize_) {
     visualize(std::move(transformed_plan));
   }
@@ -122,6 +130,36 @@ void MPPIController::setSpeedLimit(const double & speed_limit, const bool & perc
   optimizer_.setSpeedLimit(speed_limit, percentage);
 }
 
+void MPPIController::publish_optimal_path(const xt::xtensor<float, 2> & optimal_traj)
+{
+  auto & size = optimal_traj.shape()[0];
+  if (!size) {
+    return;
+  }
+
+  nav_msgs::msg::Path optimal_path;
+  optimal_path.header.stamp = clock_->now();
+  optimal_path.header.frame_id = (costmap_ros_->getGlobalFrameID()).c_str();
+
+  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 = (costmap_ros_->getGlobalFrameID()).c_str();
+
+    // position & orientation
+    pose_stamped.pose = utils::createPose(optimal_traj(i, 0), optimal_traj(i, 1), 0.);
+
+    tf2::Quaternion quaternion_tf2;
+    quaternion_tf2.setRPY(0., 0., optimal_traj(i, 2));
+    auto quaternion = tf2::toMsg(quaternion_tf2);
+    pose_stamped.pose.orientation = quaternion;
+
+    // add pose to the path
+    optimal_path.poses.push_back(pose_stamped);
+  }
+  optimal_path_pub_->publish(optimal_path);
+}
+
 }  // namespace nav2_mppi_controller
 
 #include "pluginlib/class_list_macros.hpp"