From 124843cafe13d34d193699278f7163072328bbe3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 10 Mar 2023 15:44:29 +0100 Subject: [PATCH] namespace trajectory visualizer markers --- .../nav2_mppi_controller/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 | 8 ++++---- nav2_mppi_controller/test/trajectory_visualizer_tests.cpp | 6 +++--- nav2_mppi_controller/test/utils_test.cpp | 3 ++- 6 files changed, 15 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 b728fae6d1..d6fe7b5521 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -112,8 +112,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(), "Optimimal 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..d5f69cb887 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -56,7 +56,7 @@ 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 +72,7 @@ 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 +82,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 +97,7 @@ 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..0435977665 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, "Optimimal 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, "Optimimal 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 ac230154af..37acd78e87 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)