Publish optimal trajectory as a Path message

Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io>
ros-navigation · Aug 22, 2024 · 1c56c7a · 1c56c7a
1 parent f73fa24
commit 1c56c7a
Show file tree

Hide file tree

Showing 2 changed files with 46 additions and 0 deletions.
diff --git 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
@@ -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"