populate optimal path message in add()

Signed-off-by: Alyssa Agnissan <[email protected]>

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
@@ -91,13 +93,15 @@ class TrajectoryVisualizer
     * @brief Publish the optimal trajectory in the form of a path message
     * @param trajectory Optimal trajectory
     */
-  void publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp);
+  void populate_optimal_path(
+    const xt::xtensor<float, 2> & optimal_traj,
+    const builtin_interfaces::msg::Time & cmd_stamp);
 
   /**
     * @brief Visualize the plan
     * @param plan Plan to visualize
     */
-  void visualize(const nav_msgs::msg::Path & plan, const builtin_interfaces::msg::Time & cmd_stamp);
+  void visualize(const nav_msgs::msg::Path & plan);
 
   /**
     * @brief Reset object
@@ -111,7 +115,7 @@ class TrajectoryVisualizer
   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<xt::xtensor<float, 2>> optimal_traj_;
+  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

@@ -110,8 +110,8 @@ void MPPIController::visualize(
   const builtin_interfaces::msg::Time & cmd_stamp)
 {
   trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
-  trajectory_visualizer_.visualize(std::move(transformed_plan), cmd_stamp);
+  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
+  trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 
 void MPPIController::setPlan(const nav_msgs::msg::Path & path)

nav2_mppi_controller/src/trajectory_visualizer.cpp

@@ -28,7 +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>("/local_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");
@@ -61,7 +61,9 @@ void TrajectoryVisualizer::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) {
@@ -86,7 +88,7 @@ void TrajectoryVisualizer::add(
     add_marker(i);
   }
 
-  optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>(trajectory);
+  populate_optimal_path(trajectory, cmd_stamp);
 }
 
 void TrajectoryVisualizer::add(
@@ -117,19 +119,17 @@ void TrajectoryVisualizer::reset()
 {
   marker_id_ = 0;
   points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
-  optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>();
+  optimal_path_ = std::make_unique<nav_msgs::msg::Path>();
 }
 
-void TrajectoryVisualizer::visualize(
-  const nav_msgs::msg::Path & plan,
-  const builtin_interfaces::msg::Time & cmd_stamp)
+void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
 {
   if (trajectories_publisher_->get_subscription_count() > 0) {
     trajectories_publisher_->publish(std::move(points_));
   }
 
   if (optimal_path_pub_->get_subscription_count() > 0) {
-    publish_optimal_path(cmd_stamp);
+    optimal_path_pub_->publish(std::move(optimal_path_));
   }
 
   reset();
@@ -140,33 +140,33 @@ void TrajectoryVisualizer::visualize(
   }
 }
 
-void TrajectoryVisualizer::publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp)
+void TrajectoryVisualizer::populate_optimal_path(
+  const xt::xtensor<float, 2> & optimal_traj,
+  const builtin_interfaces::msg::Time & cmd_stamp)
 {
-  auto & size = optimal_traj_->shape()[0];
+  auto & size = optimal_traj.shape()[0];
   if (size == 0) {
     return;
   }
 
-  nav_msgs::msg::Path optimal_path;
-  optimal_path.header.stamp = cmd_stamp;
-  optimal_path.header.frame_id = frame_id_;
+  optimal_path_->header.stamp = cmd_stamp;
+  optimal_path_->header.frame_id = frame_id_;
 
   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 = frame_id_;
 
     // position & orientation
-    pose_stamped.pose = utils::createPose((*optimal_traj_)(i, 0), (*optimal_traj_)(i, 1), 0.0);
+    pose_stamped.pose = utils::createPose(optimal_traj(i, 0), optimal_traj(i, 1), 0.0);
 
     tf2::Quaternion quaternion_tf2;
-    quaternion_tf2.setRPY(0., 0., (*optimal_traj_)(i, 2));
+    quaternion_tf2.setRPY(0., 0., optimal_traj(i, 2));
     pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
 
     // add pose to the path
-    optimal_path.poses.push_back(pose_stamped);
+    optimal_path_->poses.push_back(pose_stamped);
   }
-  optimal_path_pub_->publish(optimal_path);
 }
 
 }  // namespace mppi

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp

@@ -59,8 +59,7 @@ TEST(TrajectoryVisualizerTests, VisPathRepub)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "map", parameters_handler.get());
   vis.on_activate();
-  builtin_interfaces::msg::Time bogus_stamp;
-  vis.visualize(pub_path, bogus_stamp);
+  vis.visualize(pub_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
   EXPECT_EQ(recieved_path.poses.size(), 5u);
@@ -82,18 +81,18 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(optimal_trajectory, "Optimal Trajectory");
-  nav_msgs::msg::Path bogus_path;
   builtin_interfaces::msg::Time bogus_stamp;
-  vis.visualize(bogus_path, bogus_stamp);
+  vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
+  nav_msgs::msg::Path bogus_path;
+  vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
   EXPECT_EQ(recieved_msg.markers.size(), 0u);
 
   // Now populated with content, should publish
   optimal_trajectory = xt::ones<float>({20, 2});
-  vis.add(optimal_trajectory, "Optimal Trajectory");
-  vis.visualize(bogus_path, bogus_stamp);
+  vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
+  vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
 
@@ -149,8 +148,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
   vis.on_activate();
   vis.add(candidate_trajectories, "Candidate Trajectories");
   nav_msgs::msg::Path bogus_path;
-  builtin_interfaces::msg::Time bogus_stamp;
-  vis.visualize(bogus_path, bogus_stamp);
+  vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
   // 40 * 4, for 5 trajectory steps + 3 point steps
@@ -167,17 +165,17 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
 
   nav_msgs::msg::Path recieved_path;
   auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
-    "/local_plan", 10,
+    "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");
+  vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
   nav_msgs::msg::Path bogus_path;
-  vis.visualize(bogus_path, cmd_stamp);
+  vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
   EXPECT_EQ(recieved_path.poses.size(), 0u);
@@ -188,8 +186,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
     optimal_trajectory(i, 0) = static_cast<float>(i);
     optimal_trajectory(i, 1) = static_cast<float>(i);
   }
-  vis.add(optimal_trajectory, "Optimal Trajectory");
-  vis.visualize(bogus_path, cmd_stamp);
+  vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
+  vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());