Trajectory visualizer namespaces (#3467)

* namespace trajectory visualizer markers

(cherry picked from commit 124843c)

* fix linters

* fix typo

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

@@ -73,13 +73,13 @@ class TrajectoryVisualizer
     * @brief Add an optimal trajectory to visualize
     * @param trajectory Optimal trajectory
     */
-  void add(const xt::xtensor<float, 2> & trajectory);
+  void add(const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace);
 
   /**
     * @brief Add candidate trajectories to visualize
     * @param trajectories Candidate trajectories
     */
-  void add(const models::Trajectories & trajectories);
+  void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
 
   /**
     * @brief Visualize the plan

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -117,13 +117,13 @@ inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
  */
 inline visualization_msgs::msg::Marker createMarker(
   int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
-  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id)
+  const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
 {
   using visualization_msgs::msg::Marker;
   Marker marker;
   marker.header.frame_id = frame_id;
   marker.header.stamp = rclcpp::Time(0, 0);
-  marker.ns = "MarkerNS";
+  marker.ns = ns;
   marker.id = id;
   marker.type = Marker::SPHERE;
   marker.action = Marker::ADD;

nav2_mppi_controller/src/controller.cpp

@@ -104,8 +104,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
 
 void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
 {
-  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories());
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory());
+  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 

nav2_mppi_controller/src/trajectory_visualizer.cpp

@@ -56,7 +56,8 @@ void TrajectoryVisualizer::on_deactivate()
   transformed_path_pub_->on_deactivate();
 }
 
-void TrajectoryVisualizer::add(const xt::xtensor<float, 2> & trajectory)
+void TrajectoryVisualizer::add(
+  const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace)
 {
   auto & size = trajectory.shape()[0];
   if (!size) {
@@ -72,7 +73,8 @@ void TrajectoryVisualizer::add(const xt::xtensor<float, 2> & trajectory)
         utils::createScale(0.03, 0.03, 0.07) :
         utils::createScale(0.07, 0.07, 0.09);
       auto color = utils::createColor(0, component, component, 1);
-      auto marker = utils::createMarker(marker_id_++, pose, scale, color, frame_id_);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
       points_->markers.push_back(marker);
     };
 
@@ -82,7 +84,7 @@ void TrajectoryVisualizer::add(const xt::xtensor<float, 2> & trajectory)
 }
 
 void TrajectoryVisualizer::add(
-  const models::Trajectories & trajectories)
+  const models::Trajectories & trajectories, const std::string & marker_namespace)
 {
   auto & shape = trajectories.x.shape();
   const float shape_1 = static_cast<float>(shape[1]);
@@ -97,7 +99,8 @@ void TrajectoryVisualizer::add(
       auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03);
       auto scale = utils::createScale(0.03, 0.03, 0.03);
       auto color = utils::createColor(0, green_component, blue_component, 1);
-      auto marker = utils::createMarker(marker_id_++, pose, scale, color, frame_id_);
+      auto marker = utils::createMarker(
+        marker_id_++, pose, scale, color, frame_id_, marker_namespace);
 
       points_->markers.push_back(marker);
     }

nav2_mppi_controller/test/trajectory_visualizer_tests.cpp

@@ -81,7 +81,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(optimal_trajectory);
+  vis.add(optimal_trajectory, "Optimal Trajectory");
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 
@@ -90,7 +90,7 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
 
   // Now populated with content, should publish
   optimal_trajectory = xt::ones<float>({20, 2});
-  vis.add(optimal_trajectory);
+  vis.add(optimal_trajectory, "Optimal Trajectory");
   vis.visualize(bogus_path);
 
   rclcpp::spin_some(node->get_node_base_interface());
@@ -145,7 +145,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
   TrajectoryVisualizer vis;
   vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
   vis.on_activate();
-  vis.add(candidate_trajectories);
+  vis.add(candidate_trajectories, "Candidate Trajectories");
   nav_msgs::msg::Path bogus_path;
   vis.visualize(bogus_path);
 

nav2_mppi_controller/test/utils_test.cpp

@@ -80,12 +80,13 @@ TEST(UtilsTests, MarkerPopulationUtils)
   EXPECT_EQ(color.b, 3.0);
   EXPECT_EQ(color.a, 0.0);
 
-  auto marker = createMarker(999, pose, scale, color, "map");
+  auto marker = createMarker(999, pose, scale, color, "map", "ns");
   EXPECT_EQ(marker.header.frame_id, "map");
   EXPECT_EQ(marker.id, 999);
   EXPECT_EQ(marker.pose, pose);
   EXPECT_EQ(marker.scale, scale);
   EXPECT_EQ(marker.color, color);
+  EXPECT_EQ(marker.ns, "ns");
 }
 
 TEST(UtilsTests, ConversionTests)