From d8a22fa21d77ce0df8a8d4fc9693806115114b3b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 10 Mar 2023 23:45:16 +0100 Subject: [PATCH] Trajectory visualizer namespaces (#3467) * namespace trajectory visualizer markers (cherry picked from commit 124843cafe13d34d193699278f7163072328bbe3) * fix linters * fix typo --- .../tools/trajectory_visualizer.hpp | 4 ++-- .../include/nav2_mppi_controller/tools/utils.hpp | 4 ++-- nav2_mppi_controller/src/controller.cpp | 4 ++-- nav2_mppi_controller/src/trajectory_visualizer.cpp | 11 +++++++---- .../test/trajectory_visualizer_tests.cpp | 6 +++--- nav2_mppi_controller/test/utils_test.cpp | 3 ++- 6 files changed, 18 insertions(+), 14 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp index ba4dde4ca2..6ecdbdcb8d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp +++ b/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 & trajectory); + void add(const xt::xtensor & 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 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 20bea02a5d..7319e318f0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/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; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 0cf5cf2259..5f8dcffd26 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/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)); } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 320a5856d3..a6531b7d1d 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/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 & trajectory) +void TrajectoryVisualizer::add( + const xt::xtensor & trajectory, const std::string & marker_namespace) { auto & size = trajectory.shape()[0]; if (!size) { @@ -72,7 +73,8 @@ void TrajectoryVisualizer::add(const xt::xtensor & 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 & 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(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); } diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 7417856cae..7ebada2a6a 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/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({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); diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index df7dc5420a..54bcd4b787 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/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)