Publish optimal trajectory as a Path message

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -106,7 +106,9 @@ class MPPIController : public nav2_core::Controller
     * @brief Visualize trajectories
     * @param transformed_plan Transformed input plan
     */
-  void visualize(nav_msgs::msg::Path transformed_plan);
+  void visualize(
+    nav_msgs::msg::Path transformed_plan,
+    const builtin_interfaces::msg::Time & cmd_stamp);
 
   std::string name_;
   rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

@@ -79,7 +79,9 @@ class TrajectoryVisualizer
     * @brief Add an optimal trajectory to visualize
     * @param trajectory Optimal trajectory
     */
-  void add(const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace);
+  void add(
+    const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace,
+    const builtin_interfaces::msg::Time & cmd_stamp);
 
   /**
     * @brief Add candidate trajectories to visualize
@@ -103,7 +105,9 @@ class TrajectoryVisualizer
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>>
   trajectories_publisher_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_path_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> optimal_path_pub_;
 
+  std::unique_ptr<nav_msgs::msg::Path> optimal_path_;
   std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
   int marker_id_ = 0;
 

nav2_mppi_controller/src/controller.cpp

@@ -99,16 +99,18 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
 #endif
 
   if (visualize_) {
-    visualize(std::move(transformed_plan));
+    visualize(std::move(transformed_plan), cmd.header.stamp);
   }
 
   return cmd;
 }
 
-void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
+void MPPIController::visualize(
+  nav_msgs::msg::Path transformed_plan,
+  const builtin_interfaces::msg::Time & cmd_stamp)
 {
   trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
+  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 

nav2_mppi_controller/src/trajectory_visualizer.cpp

@@ -28,6 +28,7 @@ void TrajectoryVisualizer::on_configure(
   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);
   parameters_handler_ = parameters_handler;
 
   auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
@@ -42,22 +43,27 @@ void TrajectoryVisualizer::on_cleanup()
 {
   trajectories_publisher_.reset();
   transformed_path_pub_.reset();
+  optimal_path_pub_.reset();
 }
 
 void TrajectoryVisualizer::on_activate()
 {
   trajectories_publisher_->on_activate();
   transformed_path_pub_->on_activate();
+  optimal_path_pub_->on_activate();
 }
 
 void TrajectoryVisualizer::on_deactivate()
 {
   trajectories_publisher_->on_deactivate();
   transformed_path_pub_->on_deactivate();
+  optimal_path_pub_->on_deactivate();
 }
 
 void TrajectoryVisualizer::add(
-  const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace)
+  const xt::xtensor<float, 2> & trajectory,
+  const std::string & marker_namespace,
+  const builtin_interfaces::msg::Time & cmd_stamp)
 {
   auto & size = trajectory.shape()[0];
   if (!size) {
@@ -76,8 +82,21 @@ void TrajectoryVisualizer::add(
       auto marker = utils::createMarker(
         marker_id_++, pose, scale, color, frame_id_, marker_namespace);
       points_->markers.push_back(marker);
+
+      // populate optimal path
+      geometry_msgs::msg::PoseStamped pose_stamped;
+      pose_stamped.header.frame_id = frame_id_;
+      pose_stamped.pose = pose;
+
+      tf2::Quaternion quaternion_tf2;
+      quaternion_tf2.setRPY(0., 0., trajectory(i, 2));
+      pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
+
+      optimal_path_->poses.push_back(pose_stamped);
     };
 
+  optimal_path_->header.stamp = cmd_stamp;
+  optimal_path_->header.frame_id = frame_id_;
   for (size_t i = 0; i < size; i++) {
     add_marker(i);
   }
@@ -111,6 +130,7 @@ void TrajectoryVisualizer::reset()
 {
   marker_id_ = 0;
   points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
+  optimal_path_ = std::make_unique<nav_msgs::msg::Path>();
 }
 
 void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
@@ -119,6 +139,10 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
     trajectories_publisher_->publish(std::move(points_));
   }
 
+  if (optimal_path_pub_->get_subscription_count() > 0) {
+    optimal_path_pub_->publish(std::move(optimal_path_));
+  }
+
   reset();
 
   if (transformed_path_pub_->get_subscription_count() > 0) {

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp

@@ -81,7 +81,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(optimal_trajectory, "Optimal Trajectory");
+  builtin_interfaces::msg::Time bogus_stamp;
+  vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
@@ -90,7 +91,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
 
   // Now populated with content, should publish
   optimal_trajectory = xt::ones<float>({20, 2});
-  vis.add(optimal_trajectory, "Optimal Trajectory");
+  vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
   vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
@@ -153,3 +154,65 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
   // 40 * 4, for 5 trajectory steps + 3 point steps
   EXPECT_EQ(recieved_msg.markers.size(), 160u);
 }
+
+TEST(TrajectoryVisualizerTests, VisOptimalPath)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto parameters_handler = std::make_unique<ParametersHandler>(node);
+  builtin_interfaces::msg::Time cmd_stamp;
+  cmd_stamp.sec = 5;
+  cmd_stamp.nanosec = 10;
+
+  nav_msgs::msg::Path recieved_path;
+  auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
+    "optimal_trajectory", 10,
+    [&](const nav_msgs::msg::Path msg) {recieved_path = msg;});
+
+  // optimal_trajectory empty, should fail to publish
+  xt::xtensor<float, 2> optimal_trajectory;
+  TrajectoryVisualizer vis;
+  vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
+  vis.on_activate();
+  vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  nav_msgs::msg::Path bogus_path;
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+  EXPECT_EQ(recieved_path.poses.size(), 0u);
+
+  // Now populated with content, should publish
+  optimal_trajectory.resize({20, 2});
+  for (unsigned int i = 0; i != optimal_trajectory.shape()[0] - 1; i++) {
+    optimal_trajectory(i, 0) = static_cast<float>(i);
+    optimal_trajectory(i, 1) = static_cast<float>(i);
+  }
+  vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  vis.visualize(bogus_path);
+
+  rclcpp::spin_some(node->get_node_base_interface());
+
+  // Should have a 20 points path in the map frame and with same stamp than velocity command
+  EXPECT_EQ(recieved_path.poses.size(), 20u);
+  EXPECT_EQ(recieved_path.header.frame_id, "fkmap");
+  EXPECT_EQ(recieved_path.header.stamp.sec, cmd_stamp.sec);
+  EXPECT_EQ(recieved_path.header.stamp.nanosec, cmd_stamp.nanosec);
+
+  tf2::Quaternion quat;
+  for (unsigned int i = 0; i != recieved_path.poses.size() - 1; i++) {
+    // Poses should be in map frame too
+    EXPECT_EQ(recieved_path.poses[i].header.frame_id, "fkmap");
+
+    // Check positions are correct
+    EXPECT_EQ(recieved_path.poses[i].pose.position.x, static_cast<float>(i));
+    EXPECT_EQ(recieved_path.poses[i].pose.position.y, static_cast<float>(i));
+    EXPECT_EQ(recieved_path.poses[i].pose.position.z, 0.06);
+
+    // Check orientations are correct
+    quat.setRPY(0., 0., optimal_trajectory(i, 2));
+    auto expected_orientation = tf2::toMsg(quat);
+    EXPECT_EQ(recieved_path.poses[i].pose.orientation.x, expected_orientation.x);
+    EXPECT_EQ(recieved_path.poses[i].pose.orientation.y, expected_orientation.y);
+    EXPECT_EQ(recieved_path.poses[i].pose.orientation.z, expected_orientation.z);
+    EXPECT_EQ(recieved_path.poses[i].pose.orientation.w, expected_orientation.w);
+  }
+}