From 2d429c6d5d6ff97a914c205681989ec90deedfe7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 Oct 2025 12:56:39 +0200 Subject: [PATCH 01/19] Enrich MPPI visualizations Signed-off-by: Tony Najjar --- .../critics/path_align_critic.hpp | 5 + .../critics/path_angle_critic.hpp | 5 + .../critics/path_follow_critic.hpp | 5 + .../nav2_mppi_controller/optimizer.hpp | 9 ++ .../tools/trajectory_visualizer.hpp | 9 +- nav2_mppi_controller/src/controller.cpp | 7 +- .../src/critics/path_align_critic.cpp | 24 +++++ .../src/critics/path_angle_critic.cpp | 23 +++++ .../src/critics/path_follow_critic.cpp | 22 +++++ .../src/trajectory_visualizer.cpp | 99 ++++++++++++++++--- .../test/trajectory_visualizer_tests.cpp | 5 +- 11 files changed, 195 insertions(+), 18 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index f7ad2fda6a9..08bea4caed8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -18,6 +18,8 @@ #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_ros_common/publisher.hpp" namespace mppi::critics { @@ -52,6 +54,9 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; + + bool visualize_{false}; + nav2::Publisher::SharedPtr target_pose_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index b137270aba9..f7edcdc3319 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -20,6 +20,8 @@ #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_core/controller_exceptions.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_ros_common/publisher.hpp" namespace mppi::critics { @@ -81,6 +83,9 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; + + bool visualize_{false}; + nav2::Publisher::SharedPtr target_pose_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 1ccd08c32fe..ee6178cb659 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -19,6 +19,8 @@ #include "nav2_mppi_controller/critic_function.hpp" #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_ros_common/publisher.hpp" namespace mppi::critics { @@ -53,6 +55,9 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; + + bool visualize_{false}; + nav2::Publisher::SharedPtr target_pose_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 1914abde647..ba59159e1b9 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -118,6 +118,15 @@ class Optimizer */ const models::ControlSequence & getOptimalControlSequence(); + /** + * @brief Get the costs for trajectories for visualization + * @return Costs array + */ + const Eigen::ArrayXf & getCosts() const + { + return costs_; + } + /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use 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 f57d9b6b2b2..db4b09806f4 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 @@ -78,10 +78,14 @@ class TrajectoryVisualizer const builtin_interfaces::msg::Time & cmd_stamp); /** - * @brief Add candidate trajectories to visualize + * @brief Add candidate trajectories with costs to visualize * @param trajectories Candidate trajectories + * @param costs Cost array for each trajectory */ - void add(const models::Trajectories & trajectories, const std::string & marker_namespace); + void add( + const models::Trajectories & trajectories, const Eigen::ArrayXf & costs, + const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp); /** * @brief Visualize the plan @@ -109,6 +113,7 @@ class TrajectoryVisualizer size_t trajectory_step_{0}; size_t time_step_{0}; + float time_step_elevation_{0.0f}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index f8c20f4f399..1ce5c098ac1 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -140,7 +140,12 @@ void MPPIController::visualize( const builtin_interfaces::msg::Time & cmd_stamp, const Eigen::ArrayXXf & optimal_trajectory) { - trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories"); + trajectory_visualizer_.add( + optimizer_.getGeneratedTrajectories(), + optimizer_.getCosts(), + "Candidate Trajectories Cost", + cmd_stamp); + trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); trajectory_visualizer_.visualize(std::move(transformed_plan)); } diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index befa9e42eb9..eb7eebe42f3 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -30,6 +30,16 @@ void PathAlignCritic::initialize() threshold_to_consider_, "threshold_to_consider", 0.5f); getParam(use_path_orientations_, "use_path_orientations", false); + getParam(visualize_, "visualize", false); + + if (visualize_) { + auto node = parent_.lock(); + if (node) { + target_pose_pub_ = node->create_publisher( + "/PathAlignCritic/furthest_reached_path_point", 1); + target_pose_pub_->on_activate(); + } + } RCLCPP_INFO( logger_, @@ -48,6 +58,20 @@ void PathAlignCritic::score(CriticData & data) // Up to furthest only, closest path point is always 0 from path handler const size_t path_segments_count = *data.furthest_reached_path_point; float path_segments_flt = static_cast(path_segments_count); + + // Visualize target pose if enabled + if (visualize_ && path_segments_count > 0) { + auto node = parent_.lock(); + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = costmap_ros_->getGlobalFrameID(); + target_pose.header.stamp = node->get_clock()->now(); + target_pose.pose.position.x = data.path.x(path_segments_count); + target_pose.pose.position.y = data.path.y(path_segments_count); + target_pose.pose.position.z = 0.0; + target_pose.pose.orientation.w = 1.0; + target_pose_pub_->publish(target_pose); + } + if (path_segments_count < offset_from_furthest_) { return; } diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 6f42d33b042..c07dfecb4af 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -41,6 +41,7 @@ void PathAngleCritic::initialize() getParam( max_angle_to_furthest_, "max_angle_to_furthest", 0.785398f); + getParam(visualize_, "visualize", false); int mode = 0; getParam(mode, "mode", mode); @@ -53,6 +54,15 @@ void PathAngleCritic::initialize() "don't allow for reversing! Setting mode to forward preference."); } + if (visualize_) { + auto node = parent_.lock(); + if (node) { + target_pose_pub_ = node->create_publisher( + "PathAngleCritic/furthest_reached_path_point", 1); + target_pose_pub_->on_activate(); + } + } + RCLCPP_INFO( logger_, "PathAngleCritic instantiated with %d power and %f weight. Mode set to: %s", @@ -75,6 +85,19 @@ void PathAngleCritic::score(CriticData & data) const float goal_yaw = data.path.yaws(offsetted_idx); const geometry_msgs::msg::Pose & pose = data.state.pose.pose; + // Visualize target pose if enabled + if (visualize_) { + auto node = parent_.lock(); + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = costmap_ros_->getGlobalFrameID(); + target_pose.header.stamp = node->get_clock()->now(); + target_pose.pose.position.x = goal_x; + target_pose.pose.position.y = goal_y; + target_pose.pose.position.z = 0.0; + target_pose.pose.orientation.w = 1.0; + target_pose_pub_->publish(target_pose); + } + switch (mode_) { case PathAngleMode::FORWARD_PREFERENCE: if (utils::posePointAngle(pose, goal_x, goal_y, true) < max_angle_to_furthest_) { diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 240c49725d6..45d49c28812 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -29,6 +29,16 @@ void PathFollowCritic::initialize() getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); + getParam(visualize_, "visualize", false); + + if (visualize_) { + auto node = parent_.lock(); + if (node) { + target_pose_pub_ = node->create_publisher( + "/PathFollowCritic/furthest_reached_path_point", 1); + target_pose_pub_->on_activate(); + } + } } void PathFollowCritic::score(CriticData & data) @@ -60,6 +70,18 @@ void PathFollowCritic::score(CriticData & data) const auto path_x = data.path.x(offsetted_idx); const auto path_y = data.path.y(offsetted_idx); + // Visualize target pose if enabled + if (visualize_) { + auto node = parent_.lock(); + geometry_msgs::msg::PoseStamped target_pose; + target_pose.header.frame_id = costmap_ros_->getGlobalFrameID(); + target_pose.header.stamp = node->get_clock()->now(); + target_pose.pose.position.x = path_x; + target_pose.pose.position.y = path_y; + target_pose.pose.position.z = 0.0; + target_pose.pose.orientation.w = 1.0; + target_pose_pub_->publish(target_pose); + } const int && rightmost_idx = data.trajectories.x.cols() - 1; const auto last_x = data.trajectories.x.col(rightmost_idx); diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 6960b5676c6..f11c3dcb3db 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp" namespace mppi @@ -36,6 +38,7 @@ void TrajectoryVisualizer::on_configure( getParam(trajectory_step_, "trajectory_step", 5); getParam(time_step_, "time_step", 3); + getParam(time_step_elevation_, "time_step_elevation", 0.0f); reset(); } @@ -104,27 +107,95 @@ void TrajectoryVisualizer::add( } void TrajectoryVisualizer::add( - const models::Trajectories & trajectories, const std::string & marker_namespace) + const models::Trajectories & trajectories, const Eigen::ArrayXf & costs, + const std::string & marker_namespace, + const builtin_interfaces::msg::Time & cmd_stamp) { size_t n_rows = trajectories.x.rows(); size_t n_cols = trajectories.x.cols(); - const float shape_1 = static_cast(n_cols); - points_->markers.reserve(floor(n_rows / trajectory_step_) * floor(n_cols * time_step_)); + points_->markers.reserve(n_rows / trajectory_step_); + + // Use percentile-based normalization to handle outliers + // Sort costs to find percentiles + std::vector sorted_costs(costs.data(), costs.data() + costs.size()); + std::sort(sorted_costs.begin(), sorted_costs.end()); + + // Use 10th and 90th percentile for robust color mapping + size_t idx_5th = static_cast(sorted_costs.size() * 0.1); + size_t idx_95th = static_cast(sorted_costs.size() * 0.9); + + float min_cost = sorted_costs[idx_5th]; + float max_cost = sorted_costs[idx_95th]; + float cost_range = max_cost - min_cost; + + // Avoid division by zero + if (cost_range < 1e-6f) { + cost_range = 1.0f; + } for (size_t i = 0; i < n_rows; i += trajectory_step_) { - for (size_t j = 0; j < n_cols; j += time_step_) { - const float j_flt = static_cast(j); - float blue_component = 1.0f - j_flt / shape_1; - float green_component = j_flt / shape_1; - - 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_, marker_namespace); + float red_component, green_component, blue_component; + + // Normalize cost using percentile-based range, clamping outliers + float normalized_cost = (costs(i) - min_cost) / cost_range; + + // Clamp to [0, 1] range (handles outliers beyond percentiles) + normalized_cost = std::max(0.0f, std::min(1.0f, normalized_cost)); + + // Apply power function for better visual distribution + normalized_cost = std::pow(normalized_cost, 0.5f); + + // Color scheme with smooth gradient: + // Green (0.0) -> Yellow-Green (0.25) -> Yellow (0.5) -> Orange (0.75) -> Red (1.0) + // Very high outlier costs (>95th percentile) will be clamped to red + blue_component = 0.0f; + + if (normalized_cost < 0.5f) { + // Transition from Green to Yellow (0.0 - 0.5) + float t = normalized_cost * 2.0f; // Scale to [0, 1] + red_component = t; + green_component = 1.0f; + } else { + // Transition from Yellow to Red (0.5 - 1.0) + float t = (normalized_cost - 0.5f) * 2.0f; // Scale to [0, 1] + red_component = 1.0f; + green_component = 1.0f - t; + } - points_->markers.push_back(marker); + // Create line strip marker for this trajectory + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = cmd_stamp; + marker.ns = marker_namespace; + marker.id = marker_id_++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + + // Set line width + marker.scale.x = 0.01; // Line width + + // Set color for entire trajectory + marker.color.r = red_component; + marker.color.g = green_component; + marker.color.b = blue_component; + marker.color.a = 0.8f; // Slightly transparent + + // Add all points in this trajectory to the line strip + for (size_t j = 0; j < n_cols; j += time_step_) { + geometry_msgs::msg::Point point; + point.x = trajectories.x(i, j); + point.y = trajectories.y(i, j); + // Increment z by time_step_elevation_ for each time step + if (time_step_elevation_ > 0.0f) { + point.z = static_cast(j) * time_step_elevation_; + } else { + point.z = 0.0f; + } + marker.points.push_back(point); } + + 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 1228e81d60a..0d32d81b88e 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -147,10 +147,13 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12); + // Create costs array for the trajectories + Eigen::ArrayXf costs = Eigen::ArrayXf::LinSpaced(200, 0.0f, 100.0f); + TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); - vis.add(candidate_trajectories, "Candidate Trajectories"); + vis.add(candidate_trajectories, costs, "Candidate Trajectories", cmd_stamp); nav_msgs::msg::Path bogus_path; vis.visualize(bogus_path); From f0f333370e11b1518f6011df6cf291454a20bc6e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 Oct 2025 13:22:07 +0200 Subject: [PATCH 02/19] lint Signed-off-by: Tony Najjar --- .../critics/path_align_critic.hpp | 2 +- .../critics/path_angle_critic.hpp | 2 +- .../critics/path_follow_critic.hpp | 2 +- nav2_mppi_controller/src/controller.cpp | 2 +- .../src/critics/path_align_critic.cpp | 4 ++-- .../src/trajectory_visualizer.cpp | 24 +++++++++---------- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 08bea4caed8..5fa169f5507 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -54,7 +54,7 @@ class PathAlignCritic : public CriticFunction bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; - + bool visualize_{false}; nav2::Publisher::SharedPtr target_pose_pub_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index f7edcdc3319..ca72901cb8c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -83,7 +83,7 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - + bool visualize_{false}; nav2::Publisher::SharedPtr target_pose_pub_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index ee6178cb659..556242f9436 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -55,7 +55,7 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - + bool visualize_{false}; nav2::Publisher::SharedPtr target_pose_pub_; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 1ce5c098ac1..45c809ab186 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -141,7 +141,7 @@ void MPPIController::visualize( const Eigen::ArrayXXf & optimal_trajectory) { trajectory_visualizer_.add( - optimizer_.getGeneratedTrajectories(), + optimizer_.getGeneratedTrajectories(), optimizer_.getCosts(), "Candidate Trajectories Cost", cmd_stamp); diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index eb7eebe42f3..88cff554120 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -58,7 +58,7 @@ void PathAlignCritic::score(CriticData & data) // Up to furthest only, closest path point is always 0 from path handler const size_t path_segments_count = *data.furthest_reached_path_point; float path_segments_flt = static_cast(path_segments_count); - + // Visualize target pose if enabled if (visualize_ && path_segments_count > 0) { auto node = parent_.lock(); @@ -71,7 +71,7 @@ void PathAlignCritic::score(CriticData & data) target_pose.pose.orientation.w = 1.0; target_pose_pub_->publish(target_pose); } - + if (path_segments_count < offset_from_furthest_) { return; } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index f11c3dcb3db..81e1945cbdf 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -119,15 +119,15 @@ void TrajectoryVisualizer::add( // Sort costs to find percentiles std::vector sorted_costs(costs.data(), costs.data() + costs.size()); std::sort(sorted_costs.begin(), sorted_costs.end()); - + // Use 10th and 90th percentile for robust color mapping size_t idx_5th = static_cast(sorted_costs.size() * 0.1); size_t idx_95th = static_cast(sorted_costs.size() * 0.9); - + float min_cost = sorted_costs[idx_5th]; float max_cost = sorted_costs[idx_95th]; float cost_range = max_cost - min_cost; - + // Avoid division by zero if (cost_range < 1e-6f) { cost_range = 1.0f; @@ -135,21 +135,21 @@ void TrajectoryVisualizer::add( for (size_t i = 0; i < n_rows; i += trajectory_step_) { float red_component, green_component, blue_component; - + // Normalize cost using percentile-based range, clamping outliers float normalized_cost = (costs(i) - min_cost) / cost_range; - + // Clamp to [0, 1] range (handles outliers beyond percentiles) normalized_cost = std::max(0.0f, std::min(1.0f, normalized_cost)); - + // Apply power function for better visual distribution normalized_cost = std::pow(normalized_cost, 0.5f); - + // Color scheme with smooth gradient: // Green (0.0) -> Yellow-Green (0.25) -> Yellow (0.5) -> Orange (0.75) -> Red (1.0) // Very high outlier costs (>95th percentile) will be clamped to red blue_component = 0.0f; - + if (normalized_cost < 0.5f) { // Transition from Green to Yellow (0.0 - 0.5) float t = normalized_cost * 2.0f; // Scale to [0, 1] @@ -171,16 +171,16 @@ void TrajectoryVisualizer::add( marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.orientation.w = 1.0; - + // Set line width marker.scale.x = 0.01; // Line width - + // Set color for entire trajectory marker.color.r = red_component; marker.color.g = green_component; marker.color.b = blue_component; marker.color.a = 0.8f; // Slightly transparent - + // Add all points in this trajectory to the line strip for (size_t j = 0; j < n_cols; j += time_step_) { geometry_msgs::msg::Point point; @@ -194,7 +194,7 @@ void TrajectoryVisualizer::add( } marker.points.push_back(point); } - + points_->markers.push_back(marker); } } From cecc6ea3589eae4024e9b32db6a76e9fa56c538c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 Oct 2025 13:27:10 +0200 Subject: [PATCH 03/19] fix var name Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/trajectory_visualizer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 81e1945cbdf..d95e7e6695a 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -121,11 +121,11 @@ void TrajectoryVisualizer::add( std::sort(sorted_costs.begin(), sorted_costs.end()); // Use 10th and 90th percentile for robust color mapping - size_t idx_5th = static_cast(sorted_costs.size() * 0.1); - size_t idx_95th = static_cast(sorted_costs.size() * 0.9); + size_t idx_10th = static_cast(sorted_costs.size() * 0.1); + size_t idx_90th = static_cast(sorted_costs.size() * 0.9); - float min_cost = sorted_costs[idx_5th]; - float max_cost = sorted_costs[idx_95th]; + float min_cost = sorted_costs[idx_10th]; + float max_cost = sorted_costs[idx_90th]; float cost_range = max_cost - min_cost; // Avoid division by zero From 5202339037c3aa551070b44e09e13593ba59868f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 Oct 2025 14:08:02 +0200 Subject: [PATCH 04/19] fix test Signed-off-by: Tony Najjar --- nav2_mppi_controller/test/trajectory_visualizer_tests.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 0d32d81b88e..1ff1b416367 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -150,6 +150,10 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) // Create costs array for the trajectories Eigen::ArrayXf costs = Eigen::ArrayXf::LinSpaced(200, 0.0f, 100.0f); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; + TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); From 88a4adc8687903c98e917a5426c2ede2b827df16 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 24 Oct 2025 10:44:37 +0200 Subject: [PATCH 05/19] fixes Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/critic_function.hpp | 5 +++- .../critics/path_align_critic.hpp | 3 +- .../critics/path_angle_critic.hpp | 3 +- .../critics/path_follow_critic.hpp | 3 +- .../tools/trajectory_visualizer.hpp | 1 - .../src/critics/path_align_critic.cpp | 30 +++++++++++-------- .../src/critics/path_angle_critic.cpp | 26 ++++++++-------- .../src/critics/path_follow_critic.cpp | 28 +++++++++-------- .../src/trajectory_visualizer.cpp | 8 +---- 9 files changed, 57 insertions(+), 50 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp index 9b7d17aaea5..dda4a14f96e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp @@ -70,7 +70,9 @@ class CriticFunction ParametersHandler * param_handler) { parent_ = parent; - logger_ = parent_.lock()->get_logger(); + auto node = parent_.lock(); + logger_ = node->get_logger(); + clock_ = node->get_clock(); name_ = name; parent_name_ = parent_name; costmap_ros_ = costmap_ros; @@ -111,6 +113,7 @@ class CriticFunction ParametersHandler * parameters_handler_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 5fa169f5507..7b194c5667b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -19,6 +19,7 @@ #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_ros_common/publisher.hpp" namespace mppi::critics @@ -56,7 +57,7 @@ class PathAlignCritic : public CriticFunction float weight_{0}; bool visualize_{false}; - nav2::Publisher::SharedPtr target_pose_pub_; + nav2::Publisher::SharedPtr furthest_point_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index ca72901cb8c..98cfa1289fd 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -21,6 +21,7 @@ #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_core/controller_exceptions.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_ros_common/publisher.hpp" namespace mppi::critics @@ -85,7 +86,7 @@ class PathAngleCritic : public CriticFunction float weight_{0}; bool visualize_{false}; - nav2::Publisher::SharedPtr target_pose_pub_; + nav2::Publisher::SharedPtr furthest_point_pub_; }; } // namespace mppi::critics diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 556242f9436..539ea798a2e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -20,6 +20,7 @@ #include "nav2_mppi_controller/models/state.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_ros_common/publisher.hpp" namespace mppi::critics @@ -57,7 +58,7 @@ class PathFollowCritic : public CriticFunction float weight_{0}; bool visualize_{false}; - nav2::Publisher::SharedPtr target_pose_pub_; + nav2::Publisher::SharedPtr furthest_point_pub_; }; } // namespace mppi::critics 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 db4b09806f4..01e41cac805 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 @@ -113,7 +113,6 @@ class TrajectoryVisualizer size_t trajectory_step_{0}; size_t time_step_{0}; - float time_step_elevation_{0.0f}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 88cff554120..cbf06f1c240 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "nav2_mppi_controller/critics/path_align_critic.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace mppi::critics { @@ -35,9 +36,9 @@ void PathAlignCritic::initialize() if (visualize_) { auto node = parent_.lock(); if (node) { - target_pose_pub_ = node->create_publisher( - "/PathAlignCritic/furthest_reached_path_point", 1); - target_pose_pub_->on_activate(); + furthest_point_pub_ = node->create_publisher( + "PathAlignCritic/furthest_reached_path_point", 1); + furthest_point_pub_->on_activate(); } } @@ -60,16 +61,19 @@ void PathAlignCritic::score(CriticData & data) float path_segments_flt = static_cast(path_segments_count); // Visualize target pose if enabled - if (visualize_ && path_segments_count > 0) { - auto node = parent_.lock(); - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = costmap_ros_->getGlobalFrameID(); - target_pose.header.stamp = node->get_clock()->now(); - target_pose.pose.position.x = data.path.x(path_segments_count); - target_pose.pose.position.y = data.path.y(path_segments_count); - target_pose.pose.position.z = 0.0; - target_pose.pose.orientation.w = 1.0; - target_pose_pub_->publish(target_pose); + if (visualize_ && path_segments_count > 0 && + furthest_point_pub_->get_subscription_count() > 0) + { + auto furthest_point = std::make_unique(); + furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); + furthest_point->header.stamp = clock_->now(); + furthest_point->pose.position.x = data.path.x(path_segments_count); + furthest_point->pose.position.y = data.path.y(path_segments_count); + furthest_point->pose.position.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, data.path.yaws(path_segments_count)); + furthest_point->pose.orientation = tf2::toMsg(quat); + furthest_point_pub_->publish(std::move(furthest_point)); } if (path_segments_count < offset_from_furthest_) { diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index c07dfecb4af..9e3f6d7bde3 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -14,6 +14,7 @@ // limitations under the License. #include "nav2_mppi_controller/critics/path_angle_critic.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include @@ -57,9 +58,9 @@ void PathAngleCritic::initialize() if (visualize_) { auto node = parent_.lock(); if (node) { - target_pose_pub_ = node->create_publisher( + furthest_point_pub_ = node->create_publisher( "PathAngleCritic/furthest_reached_path_point", 1); - target_pose_pub_->on_activate(); + furthest_point_pub_->on_activate(); } } @@ -86,16 +87,17 @@ void PathAngleCritic::score(CriticData & data) const geometry_msgs::msg::Pose & pose = data.state.pose.pose; // Visualize target pose if enabled - if (visualize_) { - auto node = parent_.lock(); - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = costmap_ros_->getGlobalFrameID(); - target_pose.header.stamp = node->get_clock()->now(); - target_pose.pose.position.x = goal_x; - target_pose.pose.position.y = goal_y; - target_pose.pose.position.z = 0.0; - target_pose.pose.orientation.w = 1.0; - target_pose_pub_->publish(target_pose); + if (visualize_ && furthest_point_pub_->get_subscription_count() > 0) { + auto furthest_point = std::make_unique(); + furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); + furthest_point->header.stamp = clock_->now(); + furthest_point->pose.position.x = goal_x; + furthest_point->pose.position.y = goal_y; + furthest_point->pose.position.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, goal_yaw); + furthest_point->pose.orientation = tf2::toMsg(quat); + furthest_point_pub_->publish(std::move(furthest_point)); } switch (mode_) { diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 45d49c28812..9eb45625ff1 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "nav2_mppi_controller/critics/path_follow_critic.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include @@ -34,9 +35,9 @@ void PathFollowCritic::initialize() if (visualize_) { auto node = parent_.lock(); if (node) { - target_pose_pub_ = node->create_publisher( - "/PathFollowCritic/furthest_reached_path_point", 1); - target_pose_pub_->on_activate(); + furthest_point_pub_ = node->create_publisher( + "PathFollowCritic/furthest_reached_path_point", 1); + furthest_point_pub_->on_activate(); } } } @@ -71,16 +72,17 @@ void PathFollowCritic::score(CriticData & data) const auto path_x = data.path.x(offsetted_idx); const auto path_y = data.path.y(offsetted_idx); // Visualize target pose if enabled - if (visualize_) { - auto node = parent_.lock(); - geometry_msgs::msg::PoseStamped target_pose; - target_pose.header.frame_id = costmap_ros_->getGlobalFrameID(); - target_pose.header.stamp = node->get_clock()->now(); - target_pose.pose.position.x = path_x; - target_pose.pose.position.y = path_y; - target_pose.pose.position.z = 0.0; - target_pose.pose.orientation.w = 1.0; - target_pose_pub_->publish(target_pose); + if (visualize_ && furthest_point_pub_->get_subscription_count() > 0) { + auto furthest_point = std::make_unique(); + furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); + furthest_point->header.stamp = clock_->now(); + furthest_point->pose.position.x = path_x; + furthest_point->pose.position.y = path_y; + furthest_point->pose.position.z = 0.0; + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, data.path.yaws(offsetted_idx)); + furthest_point->pose.orientation = tf2::toMsg(quat); + furthest_point_pub_->publish(std::move(furthest_point)); } const int && rightmost_idx = data.trajectories.x.cols() - 1; diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index d95e7e6695a..9240b2716eb 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -38,7 +38,6 @@ void TrajectoryVisualizer::on_configure( getParam(trajectory_step_, "trajectory_step", 5); getParam(time_step_, "time_step", 3); - getParam(time_step_elevation_, "time_step_elevation", 0.0f); reset(); } @@ -186,12 +185,7 @@ void TrajectoryVisualizer::add( geometry_msgs::msg::Point point; point.x = trajectories.x(i, j); point.y = trajectories.y(i, j); - // Increment z by time_step_elevation_ for each time step - if (time_step_elevation_ > 0.0f) { - point.z = static_cast(j) * time_step_elevation_; - } else { - point.z = 0.0f; - } + point.z = 0.0f; marker.points.push_back(point); } From 8ab422843455a391a14e85e5d83337211b4aed9f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 11:09:48 +0100 Subject: [PATCH 06/19] visualization Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/controller.hpp | 4 +- .../nav2_mppi_controller/critic_data.hpp | 1 + .../nav2_mppi_controller/critic_manager.hpp | 1 + .../nav2_mppi_controller/optimizer.hpp | 15 +- .../tools/trajectory_visualizer.hpp | 12 +- nav2_mppi_controller/src/controller.cpp | 24 ++- nav2_mppi_controller/src/critic_manager.cpp | 43 +++- .../src/trajectory_visualizer.cpp | 188 +++++++++++------- .../test/critic_manager_test.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 18 +- .../test/trajectory_visualizer_tests.cpp | 9 +- nav2_mppi_controller/test/utils_test.cpp | 10 +- 12 files changed, 205 insertions(+), 122 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 1c52df96f68..c380b204f3c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -124,8 +124,8 @@ class MPPIController : public nav2_core::Controller PathHandler path_handler_; TrajectoryVisualizer trajectory_visualizer_; - bool visualize_; - bool publish_optimal_trajectory_; + bool publish_optimal_trajectory_msg_; + bool publish_optimal_footprints_; }; } // namespace nav2_mppi_controller diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index 8476dde055c..c40eb7cd04e 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -44,6 +44,7 @@ struct CriticData const geometry_msgs::msg::Pose & goal; Eigen::ArrayXf & costs; + std::optional>> individual_critics_cost; float & model_dt; bool fail_flag; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp index 237649bedde..447757d73db 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -99,6 +99,7 @@ class CriticManager nav2::Publisher::SharedPtr critics_effect_pub_; bool publish_critics_stats_; + bool visualize_per_critic_costs_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index ba59159e1b9..140da9eddfd 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -127,6 +127,19 @@ class Optimizer return costs_; } + /** + * @brief Get the per-critic costs for trajectories for visualization + * @return Vector of (critic_name, costs) pairs + */ + const std::vector> & getCriticCosts() const + { + if (critics_data_.individual_critics_cost) { + return *critics_data_.individual_critics_cost; + } + static const std::vector> empty; + return empty; + } + /** * @brief Set the maximum speed based on the speed limits callback * @param speed_limit Limit of the speed for use @@ -293,7 +306,7 @@ class Optimizer CriticData critics_data_ = { state_, generated_trajectories_, path_, goal_, - costs_, settings_.model_dt, false, nullptr, nullptr, + costs_, std::nullopt, settings_.model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; 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 01e41cac805..819d101a82d 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 @@ -80,11 +80,14 @@ class TrajectoryVisualizer /** * @brief Add candidate trajectories with costs to visualize * @param trajectories Candidate trajectories - * @param costs Cost array for each trajectory + * @param total_costs Total cost array for each trajectory + * @param individual_critics_cost Optional vector of (critic_name, cost_array) pairs for per-critic visualization + * @param cmd_stamp Timestamp for the markers */ void add( - const models::Trajectories & trajectories, const Eigen::ArrayXf & costs, - const std::string & marker_namespace, + const models::Trajectories & trajectories, + const Eigen::ArrayXf & total_costs, + const std::vector> & individual_critics_cost = {}, const builtin_interfaces::msg::Time & cmd_stamp); /** @@ -113,6 +116,9 @@ class TrajectoryVisualizer size_t trajectory_step_{0}; size_t time_step_{0}; + bool publish_optimal_trajectory_{false}; + bool publish_trajectories_with_total_cost_{false}; + bool publish_trajectories_with_individual_cost_{false}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 45c809ab186..11e8e797bd8 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -36,9 +36,11 @@ void MPPIController::configure( auto node = parent_.lock(); // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); - getParam(visualize_, "visualize", false); - getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); + // Get visualization parameters + auto getVisParam = parameters_handler_->getParamGetter(name_ + ".Visualization"); + getVisParam(publish_optimal_trajectory_msg_, "publish_optimal_trajectory_msg", false); + getVisParam(publish_optimal_footprints_, "publish_optimal_footprints", false); // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); @@ -47,7 +49,7 @@ void MPPIController::configure( parent_, name_, costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); - if (publish_optimal_trajectory_) { + if (publish_optimal_trajectory_msg_) { opt_traj_pub_ = node->create_publisher( "~/optimal_trajectory"); } @@ -115,11 +117,10 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); #endif - if (publish_optimal_trajectory_ && opt_traj_pub_->get_subscription_count() > 0) { + if (publish_optimal_trajectory_msg_ && opt_traj_pub_->get_subscription_count() > 0) { std_msgs::msg::Header trajectory_header; trajectory_header.stamp = cmd.header.stamp; trajectory_header.frame_id = costmap_ros_->getGlobalFrameID(); - auto trajectory_msg = utils::toTrajectoryMsg( optimal_trajectory, optimizer_.getOptimalControlSequence(), @@ -128,10 +129,16 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( opt_traj_pub_->publish(std::move(trajectory_msg)); } - if (visualize_) { - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + if (publish_optimal_footprints_ && opt_footprints_pub_->get_subscription_count() > 0) { + if (optimal_trajectory.size() == 0) { + optimal_trajectory = optimizer_.getOptimizedTrajectory(); + } + auto footprints_msg = createFootprintMarkers(optimal_trajectory, cmd.header); + opt_footprints_pub_->publish(std::move(footprints_msg)); } + visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + return cmd; } @@ -140,10 +147,11 @@ void MPPIController::visualize( const builtin_interfaces::msg::Time & cmd_stamp, const Eigen::ArrayXXf & optimal_trajectory) { + // Visualize candidate trajectories (with per-critic costs if available) trajectory_visualizer_.add( optimizer_.getGeneratedTrajectories(), optimizer_.getCosts(), - "Candidate Trajectories Cost", + optimizer_.getCriticCosts(), cmd_stamp); trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 305140e2e13..7fccdcf6b1e 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -37,7 +37,11 @@ void CriticManager::getParams() auto node = parent_.lock(); auto getParam = parameters_handler_->getParamGetter(name_); getParam(critic_names_, "critics", std::vector{}, ParameterType::Static); - getParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); + + // Read visualization parameters from Visualization namespace + auto getVisualizerParam = parameters_handler_->getParamGetter(name_ + ".Visualization"); + getVisualizerParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); + getVisualizerParam(visualize_per_critic_costs_, "publish_trajectories_with_individual_cost", false, ParameterType::Static); } void CriticManager::loadCritics() @@ -83,28 +87,45 @@ void CriticManager::evalTrajectoriesScores( stats_msg->costs_sum.reserve(critics_.size()); } + // Initialize per-critic costs tracking only if requested + if (visualize_per_critic_costs_) { + if (!data.individual_critics_cost) { + data.individual_critics_cost = std::vector>(); + } + data.individual_critics_cost->clear(); + data.individual_critics_cost->reserve(critics_.size()); + } + for (size_t i = 0; i < critics_.size(); ++i) { if (data.fail_flag) { break; } - // Store costs before critic evaluation + // Store costs before critic evaluation (only if needed) Eigen::ArrayXf costs_before; - if (publish_critics_stats_) { + if (visualize_per_critic_costs_ || publish_critics_stats_) { costs_before = data.costs; } critics_[i]->score(data); - // Calculate statistics if publishing is enabled - if (publish_critics_stats_) { - stats_msg->critics.push_back(critic_names_[i]); - - // Calculate sum of costs added by this individual critic + // Calculate cost contribution from this critic + if (visualize_per_critic_costs_ || publish_critics_stats_) { Eigen::ArrayXf cost_diff = data.costs - costs_before; - float costs_sum = cost_diff.sum(); - stats_msg->costs_sum.push_back(costs_sum); - stats_msg->changed.push_back(costs_sum != 0.0f); + + if (visualize_per_critic_costs_) { + data.individual_critics_cost->emplace_back(critic_names_[i], cost_diff); + } + + // Calculate statistics if publishing is enabled + if (publish_critics_stats_) { + stats_msg->critics.push_back(critic_names_[i]); + + // Calculate sum of costs added by this individual critic + float costs_sum = cost_diff.sum(); + stats_msg->costs_sum.push_back(costs_sum); + stats_msg->changed.push_back(costs_sum != 0.0f); + } } } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 9240b2716eb..c6d831b788e 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -34,10 +34,13 @@ void TrajectoryVisualizer::on_configure( optimal_path_pub_ = node->create_publisher("~/optimal_path"); parameters_handler_ = parameters_handler; - auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer"); + auto getParam = parameters_handler->getParamGetter(name + ".Visualization"); getParam(trajectory_step_, "trajectory_step", 5); getParam(time_step_, "time_step", 3); + getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); + getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); + getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", false); reset(); } @@ -106,90 +109,125 @@ void TrajectoryVisualizer::add( } void TrajectoryVisualizer::add( - const models::Trajectories & trajectories, const Eigen::ArrayXf & costs, - const std::string & marker_namespace, + const models::Trajectories & trajectories, + const Eigen::ArrayXf & total_costs, + const std::vector> & individual_critics_cost, const builtin_interfaces::msg::Time & cmd_stamp) { + // Check if we should visualize per-critic costs + bool visualize_per_critic = !individual_critics_cost.empty() && + publish_trajectories_with_individual_cost_ && + trajectories_publisher_->get_subscription_count() > 0; + size_t n_rows = trajectories.x.rows(); size_t n_cols = trajectories.x.cols(); points_->markers.reserve(n_rows / trajectory_step_); - // Use percentile-based normalization to handle outliers - // Sort costs to find percentiles - std::vector sorted_costs(costs.data(), costs.data() + costs.size()); - std::sort(sorted_costs.begin(), sorted_costs.end()); - - // Use 10th and 90th percentile for robust color mapping - size_t idx_10th = static_cast(sorted_costs.size() * 0.1); - size_t idx_90th = static_cast(sorted_costs.size() * 0.9); - - float min_cost = sorted_costs[idx_10th]; - float max_cost = sorted_costs[idx_90th]; - float cost_range = max_cost - min_cost; - - // Avoid division by zero - if (cost_range < 1e-6f) { - cost_range = 1.0f; - } + // Helper lambda to create a trajectory marker with given costs and namespace + auto create_trajectory_markers = [&]( + const Eigen::ArrayXf & costs, + const std::string & ns, + bool use_collision_coloring) { + + // Find min/max cost for normalization + float min_cost = std::numeric_limits::max(); + float max_cost = std::numeric_limits::lowest(); + + for (Eigen::Index i = 0; i < costs.size(); ++i) { + // Skip collision trajectories for min/max calculation if using collision coloring + if (use_collision_coloring && costs(i) >= 1000000.0f) { + continue; + } + min_cost = std::min(min_cost, costs(i)); + max_cost = std::max(max_cost, costs(i)); + } + + float cost_range = max_cost - min_cost; + + // Avoid division by zero + if (cost_range < 1e-6f) { + cost_range = 1.0f; + } - for (size_t i = 0; i < n_rows; i += trajectory_step_) { - float red_component, green_component, blue_component; - - // Normalize cost using percentile-based range, clamping outliers - float normalized_cost = (costs(i) - min_cost) / cost_range; - - // Clamp to [0, 1] range (handles outliers beyond percentiles) - normalized_cost = std::max(0.0f, std::min(1.0f, normalized_cost)); - - // Apply power function for better visual distribution - normalized_cost = std::pow(normalized_cost, 0.5f); - - // Color scheme with smooth gradient: - // Green (0.0) -> Yellow-Green (0.25) -> Yellow (0.5) -> Orange (0.75) -> Red (1.0) - // Very high outlier costs (>95th percentile) will be clamped to red - blue_component = 0.0f; - - if (normalized_cost < 0.5f) { - // Transition from Green to Yellow (0.0 - 0.5) - float t = normalized_cost * 2.0f; // Scale to [0, 1] - red_component = t; - green_component = 1.0f; - } else { - // Transition from Yellow to Red (0.5 - 1.0) - float t = (normalized_cost - 0.5f) * 2.0f; // Scale to [0, 1] - red_component = 1.0f; - green_component = 1.0f - t; + for (size_t i = 0; i < n_rows; i += trajectory_step_) { + float red_component, green_component, blue_component; + + // Check if this trajectory is in collision (cost >= 1000000) + bool in_collision = use_collision_coloring && costs(i) >= 1000000.0f; + + // Check if cost is zero (no contribution from this critic) + bool zero_cost = std::abs(costs(i)) < 1e-6f; + + if (in_collision) { + // Fixed red color for collision trajectories + red_component = 1.0f; + green_component = 0.0f; + blue_component = 0.0f; + } else if (zero_cost) { + // Gray color for zero cost (no contribution) + red_component = 0.5f; + green_component = 0.5f; + blue_component = 0.5f; + } else { + // Normal gradient for non-collision trajectories + float normalized_cost = (costs(i) - min_cost) / cost_range; + normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f); + + // Color scheme: Green (low cost) -> Yellow -> Red (high cost) + blue_component = 0.0f; + if (normalized_cost < 0.5f) { + // Green to Yellow (0.0 - 0.5) + red_component = normalized_cost * 2.0f; + green_component = 1.0f; + } else { + // Yellow to Red (0.5 - 1.0) + red_component = 1.0f; + green_component = 2.0f * (1.0f - normalized_cost); + } + } + + // Create line strip marker for this trajectory + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = cmd_stamp; + marker.ns = ns; + marker.id = marker_id_++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; // Line width + marker.color.r = red_component; + marker.color.g = green_component; + marker.color.b = blue_component; + marker.color.a = 1.0; + + // Add all points in this trajectory to the line strip + for (size_t j = 0; j < n_cols; j += time_step_) { + geometry_msgs::msg::Point point; + point.x = trajectories.x(i, j); + point.y = trajectories.y(i, j); + point.z = 0.0f; + marker.points.push_back(point); + } + + points_->markers.push_back(marker); } + }; - // Create line strip marker for this trajectory - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id_; - marker.header.stamp = cmd_stamp; - marker.ns = marker_namespace; - marker.id = marker_id_++; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - - // Set line width - marker.scale.x = 0.01; // Line width - - // Set color for entire trajectory - marker.color.r = red_component; - marker.color.g = green_component; - marker.color.b = blue_component; - marker.color.a = 0.8f; // Slightly transparent - - // Add all points in this trajectory to the line strip - for (size_t j = 0; j < n_cols; j += time_step_) { - geometry_msgs::msg::Point point; - point.x = trajectories.x(i, j); - point.y = trajectories.y(i, j); - point.z = 0.0f; - marker.points.push_back(point); + // If visualizing per-critic costs + if (visualize_per_critic) { + // Visualize total costs if requested + if (publish_trajectories_with_total_cost_) { + create_trajectory_markers(total_costs, "Total Costs", false); } - points_->markers.push_back(marker); + // Visualize each critic's contribution + for (const auto & [critic_name, costs] : individual_critics_cost) { + create_trajectory_markers(costs, critic_name, true); // Use collision coloring + } + } else { + // Simple visualization with just total costs + create_trajectory_markers(total_costs, "Total Costs", false); } } @@ -206,7 +244,7 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) trajectories_publisher_->publish(std::move(points_)); } - if (optimal_path_pub_->get_subscription_count() > 0) { + if (publish_optimal_trajectory_ && optimal_path_pub_->get_subscription_count() > 0) { optimal_path_pub_->publish(std::move(optimal_path_)); } diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index ff0116ed7de..4acb3bbec6a 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -114,7 +114,7 @@ TEST(CriticManagerTests, BasicCriticOperations) Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 88e823019a7..de9f72357f3 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -79,7 +79,7 @@ TEST(CriticTests, ConstraintsCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -227,7 +227,7 @@ TEST(CriticTests, GoalAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -290,7 +290,7 @@ TEST(CriticTests, GoalCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); @@ -346,7 +346,7 @@ TEST(CriticTests, PathAngleCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -467,7 +467,7 @@ TEST(CriticTests, PreferForwardCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -526,7 +526,7 @@ TEST(CriticTests, TwirlingCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -593,7 +593,7 @@ TEST(CriticTests, PathFollowCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -647,7 +647,7 @@ TEST(CriticTests, PathAlignCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally @@ -764,7 +764,7 @@ TEST(CriticTests, VelocityDeadbandCritic) Eigen::ArrayXf costs = Eigen::ArrayXf::Zero(1000); float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; data.motion_model = std::make_shared(); diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index 1ff1b416367..a02077d1b1c 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -147,17 +147,12 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) candidate_trajectories.y = Eigen::ArrayXXf::Ones(200, 12); candidate_trajectories.yaws = Eigen::ArrayXXf::Ones(200, 12); - // Create costs array for the trajectories - Eigen::ArrayXf costs = Eigen::ArrayXf::LinSpaced(200, 0.0f, 100.0f); - - builtin_interfaces::msg::Time cmd_stamp; - cmd_stamp.sec = 5; - cmd_stamp.nanosec = 10; + Eigen::ArrayXf costs = Eigen::ArrayXf::Random(200); TrajectoryVisualizer vis; vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); vis.on_activate(); - vis.add(candidate_trajectories, costs, "Candidate Trajectories", cmd_stamp); + vis.add(candidate_trajectories, costs, {}, cmd_stamp); 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 70733e37db4..2f0ce45b4f7 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -181,7 +181,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change @@ -191,7 +191,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // Attempt to set if not set already with no other information, should fail CriticData data2 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -210,7 +210,7 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); } @@ -226,7 +226,7 @@ TEST(UtilsTests, findPathCosts) float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Test not set if already set, should not change @@ -239,7 +239,7 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( From d1092e7904e3ca830372fc867d9729145ad127f7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 11:11:17 +0100 Subject: [PATCH 07/19] lint Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critic_manager.cpp | 7 +- .../src/trajectory_visualizer.cpp | 137 +++++++++--------- .../test/critic_manager_test.cpp | 3 +- nav2_mppi_controller/test/utils_test.cpp | 15 +- 4 files changed, 85 insertions(+), 77 deletions(-) diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 7fccdcf6b1e..495f98fafa9 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -37,11 +37,12 @@ void CriticManager::getParams() auto node = parent_.lock(); auto getParam = parameters_handler_->getParamGetter(name_); getParam(critic_names_, "critics", std::vector{}, ParameterType::Static); - + // Read visualization parameters from Visualization namespace auto getVisualizerParam = parameters_handler_->getParamGetter(name_ + ".Visualization"); getVisualizerParam(publish_critics_stats_, "publish_critics_stats", false, ParameterType::Static); - getVisualizerParam(visualize_per_critic_costs_, "publish_trajectories_with_individual_cost", false, ParameterType::Static); + getVisualizerParam(visualize_per_critic_costs_, "publish_trajectories_with_individual_cost", + false, ParameterType::Static); } void CriticManager::loadCritics() @@ -112,7 +113,7 @@ void CriticManager::evalTrajectoriesScores( // Calculate cost contribution from this critic if (visualize_per_critic_costs_ || publish_critics_stats_) { Eigen::ArrayXf cost_diff = data.costs - costs_before; - + if (visualize_per_critic_costs_) { data.individual_critics_cost->emplace_back(critic_names_[i], cost_diff); } diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index c6d831b788e..9482d6a25f9 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -40,7 +40,8 @@ void TrajectoryVisualizer::on_configure( getParam(time_step_, "time_step", 3); getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); - getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", false); + getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", + false); reset(); } @@ -115,9 +116,9 @@ void TrajectoryVisualizer::add( const builtin_interfaces::msg::Time & cmd_stamp) { // Check if we should visualize per-critic costs - bool visualize_per_critic = !individual_critics_cost.empty() && - publish_trajectories_with_individual_cost_ && - trajectories_publisher_->get_subscription_count() > 0; + bool visualize_per_critic = !individual_critics_cost.empty() && + publish_trajectories_with_individual_cost_ && + trajectories_publisher_->get_subscription_count() > 0; size_t n_rows = trajectories.x.rows(); size_t n_cols = trajectories.x.cols(); @@ -128,91 +129,91 @@ void TrajectoryVisualizer::add( const Eigen::ArrayXf & costs, const std::string & ns, bool use_collision_coloring) { - + // Find min/max cost for normalization - float min_cost = std::numeric_limits::max(); - float max_cost = std::numeric_limits::lowest(); - - for (Eigen::Index i = 0; i < costs.size(); ++i) { + float min_cost = std::numeric_limits::max(); + float max_cost = std::numeric_limits::lowest(); + + for (Eigen::Index i = 0; i < costs.size(); ++i) { // Skip collision trajectories for min/max calculation if using collision coloring - if (use_collision_coloring && costs(i) >= 1000000.0f) { - continue; + if (use_collision_coloring && costs(i) >= 1000000.0f) { + continue; + } + min_cost = std::min(min_cost, costs(i)); + max_cost = std::max(max_cost, costs(i)); } - min_cost = std::min(min_cost, costs(i)); - max_cost = std::max(max_cost, costs(i)); - } - - float cost_range = max_cost - min_cost; - + + float cost_range = max_cost - min_cost; + // Avoid division by zero - if (cost_range < 1e-6f) { - cost_range = 1.0f; - } + if (cost_range < 1e-6f) { + cost_range = 1.0f; + } - for (size_t i = 0; i < n_rows; i += trajectory_step_) { - float red_component, green_component, blue_component; + for (size_t i = 0; i < n_rows; i += trajectory_step_) { + float red_component, green_component, blue_component; // Check if this trajectory is in collision (cost >= 1000000) - bool in_collision = use_collision_coloring && costs(i) >= 1000000.0f; - + bool in_collision = use_collision_coloring && costs(i) >= 1000000.0f; + // Check if cost is zero (no contribution from this critic) - bool zero_cost = std::abs(costs(i)) < 1e-6f; - - if (in_collision) { + bool zero_cost = std::abs(costs(i)) < 1e-6f; + + if (in_collision) { // Fixed red color for collision trajectories - red_component = 1.0f; - green_component = 0.0f; - blue_component = 0.0f; - } else if (zero_cost) { + red_component = 1.0f; + green_component = 0.0f; + blue_component = 0.0f; + } else if (zero_cost) { // Gray color for zero cost (no contribution) - red_component = 0.5f; - green_component = 0.5f; - blue_component = 0.5f; - } else { + red_component = 0.5f; + green_component = 0.5f; + blue_component = 0.5f; + } else { // Normal gradient for non-collision trajectories - float normalized_cost = (costs(i) - min_cost) / cost_range; - normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f); + float normalized_cost = (costs(i) - min_cost) / cost_range; + normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f); // Color scheme: Green (low cost) -> Yellow -> Red (high cost) - blue_component = 0.0f; - if (normalized_cost < 0.5f) { + blue_component = 0.0f; + if (normalized_cost < 0.5f) { // Green to Yellow (0.0 - 0.5) - red_component = normalized_cost * 2.0f; - green_component = 1.0f; - } else { + red_component = normalized_cost * 2.0f; + green_component = 1.0f; + } else { // Yellow to Red (0.5 - 1.0) - red_component = 1.0f; - green_component = 2.0f * (1.0f - normalized_cost); + red_component = 1.0f; + green_component = 2.0f * (1.0f - normalized_cost); + } } - } // Create line strip marker for this trajectory - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id_; - marker.header.stamp = cmd_stamp; - marker.ns = ns; - marker.id = marker_id_++; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.01; // Line width - marker.color.r = red_component; - marker.color.g = green_component; - marker.color.b = blue_component; - marker.color.a = 1.0; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = cmd_stamp; + marker.ns = ns; + marker.id = marker_id_++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; // Line width + marker.color.r = red_component; + marker.color.g = green_component; + marker.color.b = blue_component; + marker.color.a = 1.0; // Add all points in this trajectory to the line strip - for (size_t j = 0; j < n_cols; j += time_step_) { - geometry_msgs::msg::Point point; - point.x = trajectories.x(i, j); - point.y = trajectories.y(i, j); - point.z = 0.0f; - marker.points.push_back(point); + for (size_t j = 0; j < n_cols; j += time_step_) { + geometry_msgs::msg::Point point; + point.x = trajectories.x(i, j); + point.y = trajectories.y(i, j); + point.z = 0.0f; + marker.points.push_back(point); + } + + points_->markers.push_back(marker); } - - points_->markers.push_back(marker); - } - }; + }; // If visualizing per-critic costs if (visualize_per_critic) { diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp index 4acb3bbec6a..8feda3eec38 100644 --- a/nav2_mppi_controller/test/critic_manager_test.cpp +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -114,7 +114,8 @@ TEST(CriticManagerTests, BasicCriticOperations) Eigen::ArrayXf costs; float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; data.fail_flag = true; diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 2f0ce45b4f7..c02b719c508 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -181,7 +181,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Attempt to set furthest point if notionally set, should not change @@ -191,7 +192,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) // Attempt to set if not set already with no other information, should fail CriticData data2 = - {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references setPathFurthestPointIfNotSet(data2); EXPECT_EQ(data2.furthest_reached_path_point, 0); @@ -210,7 +212,8 @@ TEST(UtilsTests, FurthestAndClosestReachedPoint) path = toTensor(plan); CriticData data3 = - {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references EXPECT_EQ(findPathFurthestReachedPoint(data3), 5); } @@ -226,7 +229,8 @@ TEST(UtilsTests, findPathCosts) float model_dt = 0.1; CriticData data = - {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references // Test not set if already set, should not change @@ -239,7 +243,8 @@ TEST(UtilsTests, findPathCosts) EXPECT_EQ(data.path_pts_valid->size(), 10u); CriticData data3 = - {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, nullptr, + {state, generated_trajectories, path, goal, costs, std::nullopt, model_dt, false, nullptr, + nullptr, std::nullopt, std::nullopt}; /// Caution, keep references auto costmap_ros = std::make_shared( From 081e54dc1c16e0eb3271b11d48cca8306c50e4d8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 11:38:54 +0100 Subject: [PATCH 08/19] Add optimal footprint visualization Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/controller.hpp | 6 +- .../tools/trajectory_visualizer.hpp | 29 ++++- nav2_mppi_controller/src/controller.cpp | 19 +--- .../src/trajectory_visualizer.cpp | 102 +++++++++++++++++- .../test/trajectory_visualizer_tests.cpp | 3 + 5 files changed, 137 insertions(+), 22 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index c380b204f3c..07c09df8609 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -104,12 +104,12 @@ class MPPIController : public nav2_core::Controller /** * @brief Visualize trajectories * @param transformed_plan Transformed input plan - * @param cmd_stamp Command stamp + * @param header Message header * @param optimal_trajectory Optimal trajectory, if already computed */ void visualize( nav_msgs::msg::Path transformed_plan, - const builtin_interfaces::msg::Time & cmd_stamp, + const std_msgs::msg::Header & header, const Eigen::ArrayXXf & optimal_trajectory); std::string name_; @@ -118,14 +118,12 @@ class MPPIController : public nav2_core::Controller std::shared_ptr costmap_ros_; std::shared_ptr tf_buffer_; nav2::Publisher::SharedPtr opt_traj_pub_; - std::unique_ptr parameters_handler_; Optimizer optimizer_; PathHandler path_handler_; TrajectoryVisualizer trajectory_visualizer_; bool publish_optimal_trajectory_msg_; - bool publish_optimal_footprints_; }; } // namespace nav2_mppi_controller 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 819d101a82d..dc3a8b8d3a7 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 @@ -22,6 +22,8 @@ #include "nav_msgs/msg/path.hpp" #include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "nav2_mppi_controller/tools/parameters_handler.hpp" @@ -88,13 +90,32 @@ class TrajectoryVisualizer const models::Trajectories & trajectories, const Eigen::ArrayXf & total_costs, const std::vector> & individual_critics_cost = {}, - const builtin_interfaces::msg::Time & cmd_stamp); + const builtin_interfaces::msg::Time & cmd_stamp = builtin_interfaces::msg::Time()); /** - * @brief Visualize the plan + * @brief Visualize the plan and optimal footprints * @param plan Plan to visualize + * @param optimal_trajectory Optimal trajectory for footprint visualization + * @param header Message header for footprints + * @param costmap_ros Costmap ROS pointer for footprint and frame information + */ + void visualize( + const nav_msgs::msg::Path & plan, + const Eigen::ArrayXXf & optimal_trajectory = Eigen::ArrayXXf(), + const std_msgs::msg::Header & header = std_msgs::msg::Header(), + std::shared_ptr costmap_ros = nullptr); + + /** + * @brief Create footprint markers from trajectory + * @param trajectory Optimal trajectory + * @param header Message header + * @param costmap_ros Costmap ROS pointer for footprint and frame information + * @return MarkerArray containing footprint polygons */ - void visualize(const nav_msgs::msg::Path & plan); + visualization_msgs::msg::MarkerArray createFootprintMarkers( + const Eigen::ArrayXXf & trajectory, + const std_msgs::msg::Header & header, + std::shared_ptr costmap_ros); /** * @brief Reset object @@ -107,6 +128,7 @@ class TrajectoryVisualizer trajectories_publisher_; nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr optimal_path_pub_; + nav2::Publisher::SharedPtr optimal_footprints_pub_; std::unique_ptr optimal_path_; std::unique_ptr points_; @@ -119,6 +141,7 @@ class TrajectoryVisualizer bool publish_optimal_trajectory_{false}; bool publish_trajectories_with_total_cost_{false}; bool publish_trajectories_with_individual_cost_{false}; + bool publish_optimal_footprints_{false}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 11e8e797bd8..6707fe222df 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -40,7 +40,6 @@ void MPPIController::configure( // Get visualization parameters auto getVisParam = parameters_handler_->getParamGetter(name_ + ".Visualization"); getVisParam(publish_optimal_trajectory_msg_, "publish_optimal_trajectory_msg", false); - getVisParam(publish_optimal_footprints_, "publish_optimal_footprints", false); // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); @@ -129,22 +128,14 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( opt_traj_pub_->publish(std::move(trajectory_msg)); } - if (publish_optimal_footprints_ && opt_footprints_pub_->get_subscription_count() > 0) { - if (optimal_trajectory.size() == 0) { - optimal_trajectory = optimizer_.getOptimizedTrajectory(); - } - auto footprints_msg = createFootprintMarkers(optimal_trajectory, cmd.header); - opt_footprints_pub_->publish(std::move(footprints_msg)); - } - - visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory); + visualize(std::move(transformed_plan), cmd.header, optimal_trajectory); return cmd; } void MPPIController::visualize( nav_msgs::msg::Path transformed_plan, - const builtin_interfaces::msg::Time & cmd_stamp, + const std_msgs::msg::Header & header, const Eigen::ArrayXXf & optimal_trajectory) { // Visualize candidate trajectories (with per-critic costs if available) @@ -152,10 +143,10 @@ void MPPIController::visualize( optimizer_.getGeneratedTrajectories(), optimizer_.getCosts(), optimizer_.getCriticCosts(), - cmd_stamp); + header.stamp); - trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp); - trajectory_visualizer_.visualize(std::move(transformed_plan)); + trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", header.stamp); + trajectory_visualizer_.visualize(std::move(transformed_plan), optimal_trajectory, header, costmap_ros_); } void MPPIController::setPlan(const nav_msgs::msg::Path & path) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 9482d6a25f9..5d08027e0e1 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -42,6 +42,12 @@ void TrajectoryVisualizer::on_configure( getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", false); + getParam(publish_optimal_footprints_, "publish_optimal_footprints", false); + + if (publish_optimal_footprints_) { + optimal_footprints_pub_ = node->create_publisher( + "~/optimal_footprints", rclcpp::SystemDefaultsQoS()); + } reset(); } @@ -51,6 +57,7 @@ void TrajectoryVisualizer::on_cleanup() trajectories_publisher_.reset(); transformed_path_pub_.reset(); optimal_path_pub_.reset(); + optimal_footprints_pub_.reset(); } void TrajectoryVisualizer::on_activate() @@ -58,6 +65,9 @@ void TrajectoryVisualizer::on_activate() trajectories_publisher_->on_activate(); transformed_path_pub_->on_activate(); optimal_path_pub_->on_activate(); + if (optimal_footprints_pub_) { + optimal_footprints_pub_->on_activate(); + } } void TrajectoryVisualizer::on_deactivate() @@ -65,6 +75,9 @@ void TrajectoryVisualizer::on_deactivate() trajectories_publisher_->on_deactivate(); transformed_path_pub_->on_deactivate(); optimal_path_pub_->on_deactivate(); + if (optimal_footprints_pub_) { + optimal_footprints_pub_->on_deactivate(); + } } void TrajectoryVisualizer::add( @@ -239,7 +252,11 @@ void TrajectoryVisualizer::reset() optimal_path_ = std::make_unique(); } -void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) +void TrajectoryVisualizer::visualize( + const nav_msgs::msg::Path & plan, + const Eigen::ArrayXXf & optimal_trajectory, + const std_msgs::msg::Header & header, + std::shared_ptr costmap_ros) { if (trajectories_publisher_->get_subscription_count() > 0) { trajectories_publisher_->publish(std::move(points_)); @@ -249,6 +266,15 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) optimal_path_pub_->publish(std::move(optimal_path_)); } + // Publish optimal footprints if enabled + if (publish_optimal_footprints_ && optimal_footprints_pub_ && + optimal_footprints_pub_->get_subscription_count() > 0 && + costmap_ros != nullptr && optimal_trajectory.rows() > 0) + { + auto footprints_msg = createFootprintMarkers(optimal_trajectory, header, costmap_ros); + optimal_footprints_pub_->publish(std::move(footprints_msg)); + } + reset(); if (transformed_path_pub_->get_subscription_count() > 0) { @@ -257,4 +283,78 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan) } } +visualization_msgs::msg::MarkerArray TrajectoryVisualizer::createFootprintMarkers( + const Eigen::ArrayXXf & trajectory, + const std_msgs::msg::Header & header, + std::shared_ptr costmap_ros) +{ + visualization_msgs::msg::MarkerArray marker_array; + + if (trajectory.rows() == 0) { + return marker_array; + } + + // Get robot footprint from costmap + auto robot_footprint = costmap_ros->getRobotFootprint(); + + // Skip if footprint is empty or very small + if (robot_footprint.size() < 3) { + return marker_array; + } + + // Create header with costmap frame + std_msgs::msg::Header costmap_header; + costmap_header.stamp = header.stamp; + costmap_header.frame_id = costmap_ros->getGlobalFrameID(); + + // Sample every N points to avoid too many markers (adjust as needed) + const int footprint_downsample_factor = std::max(1, static_cast(trajectory.rows() / 20)); + + int marker_id = 0; + for (int i = 0; i < trajectory.rows(); i += footprint_downsample_factor) { + double x = trajectory(i, 0); + double y = trajectory(i, 1); + double theta = trajectory(i, 2); + + // Create oriented footprint + geometry_msgs::msg::PolygonStamped oriented_footprint; + oriented_footprint.header = costmap_header; + nav2_costmap_2d::transformFootprint(x, y, theta, robot_footprint, oriented_footprint); + + // Create marker for this footprint + visualization_msgs::msg::Marker marker; + marker.header = costmap_header; + marker.ns = "optimal_footprints"; + marker.id = marker_id++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + + // Set marker scale and color + marker.scale.x = 0.02; // Line width + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 0.8; + + // Add footprint points to marker + for (const auto & point : oriented_footprint.polygon.points) { + geometry_msgs::msg::Point p; + p.x = point.x; + p.y = point.y; + p.z = 0.0; + marker.points.push_back(p); + } + + // Close the polygon by adding the first point again + if (!marker.points.empty()) { + marker.points.push_back(marker.points[0]); + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace mppi diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp index a02077d1b1c..571e65e485e 100644 --- a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -133,6 +133,9 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories) auto node = std::make_shared("my_node"); std::string name = "test"; auto parameters_handler = std::make_unique(node, name); + builtin_interfaces::msg::Time cmd_stamp; + cmd_stamp.sec = 5; + cmd_stamp.nanosec = 10; visualization_msgs::msg::MarkerArray received_msg; auto my_sub = node->create_subscription( From b9f522e4352a0f0ea0e78b10dff66228350cb7a1 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 11:11:17 +0100 Subject: [PATCH 09/19] lint Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/trajectory_visualizer.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 5d08027e0e1..b43a60a6add 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -42,12 +42,6 @@ void TrajectoryVisualizer::on_configure( getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", false); - getParam(publish_optimal_footprints_, "publish_optimal_footprints", false); - - if (publish_optimal_footprints_) { - optimal_footprints_pub_ = node->create_publisher( - "~/optimal_footprints", rclcpp::SystemDefaultsQoS()); - } reset(); } From 9c23e5739f014b691d87d4b5d23e22c1531855ee Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 11:38:54 +0100 Subject: [PATCH 10/19] Add optimal footprint visualization Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/controller.hpp | 14 - .../tools/trajectory_visualizer.hpp | 66 +++- nav2_mppi_controller/src/controller.cpp | 82 ++-- .../src/trajectory_visualizer.cpp | 352 +++++++++++------- 4 files changed, 294 insertions(+), 220 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp index 07c09df8609..43399315af7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -101,29 +101,15 @@ class MPPIController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: - /** - * @brief Visualize trajectories - * @param transformed_plan Transformed input plan - * @param header Message header - * @param optimal_trajectory Optimal trajectory, if already computed - */ - void visualize( - nav_msgs::msg::Path transformed_plan, - const std_msgs::msg::Header & header, - const Eigen::ArrayXXf & optimal_trajectory); - std::string name_; nav2::LifecycleNode::WeakPtr parent_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; std::shared_ptr costmap_ros_; std::shared_ptr tf_buffer_; - nav2::Publisher::SharedPtr opt_traj_pub_; std::unique_ptr parameters_handler_; Optimizer optimizer_; PathHandler path_handler_; TrajectoryVisualizer trajectory_visualizer_; - - bool publish_optimal_trajectory_msg_; }; } // namespace nav2_mppi_controller 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 dc3a8b8d3a7..f0e027da2b2 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 @@ -21,6 +21,7 @@ #include #include "nav_msgs/msg/path.hpp" +#include "nav2_msgs/msg/trajectory.hpp" #include "nav2_ros_common/lifecycle_node.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/footprint.hpp" @@ -29,6 +30,7 @@ #include "nav2_mppi_controller/tools/parameters_handler.hpp" #include "nav2_mppi_controller/tools/utils.hpp" #include "nav2_mppi_controller/models/trajectories.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" namespace mppi { @@ -93,17 +95,52 @@ class TrajectoryVisualizer const builtin_interfaces::msg::Time & cmd_stamp = builtin_interfaces::msg::Time()); /** - * @brief Visualize the plan and optimal footprints - * @param plan Plan to visualize - * @param optimal_trajectory Optimal trajectory for footprint visualization - * @param header Message header for footprints - * @param costmap_ros Costmap ROS pointer for footprint and frame information + * @brief Visualize all trajectory data in one call + * @param plan Transformed plan to visualize + * @param optimal_trajectory Optimal trajectory + * @param control_sequence Control sequence for optimal trajectory + * @param model_dt Model time step + * @param stamp Timestamp for the visualization + * @param costmap_ros Costmap ROS pointer + * @param candidate_trajectories Generated candidate trajectories + * @param costs Total costs for each trajectory + * @param critic_costs Per-critic costs for each trajectory */ void visualize( - const nav_msgs::msg::Path & plan, - const Eigen::ArrayXXf & optimal_trajectory = Eigen::ArrayXXf(), - const std_msgs::msg::Header & header = std_msgs::msg::Header(), - std::shared_ptr costmap_ros = nullptr); + nav_msgs::msg::Path plan, + const Eigen::ArrayXXf & optimal_trajectory, + const models::ControlSequence & control_sequence, + float model_dt, + const builtin_interfaces::msg::Time & stamp, + std::shared_ptr costmap_ros, + const models::Trajectories & candidate_trajectories, + const Eigen::ArrayXf & costs, + const std::vector> & critic_costs); + + /** + * @brief Visualize without optimizer (for testing) + * @param plan Transformed plan to visualize + */ + void visualize(nav_msgs::msg::Path plan); + + /** + * @brief Reset object + */ + void reset(); + +protected: + /** + * @brief Create trajectory markers with cost-based coloring + * @param trajectories Set of trajectories to visualize + * @param costs Cost array for each trajectory + * @param ns Namespace for the markers + * @param cmd_stamp Timestamp for the markers + */ + void createTrajectoryMarkers( + const models::Trajectories & trajectories, + const Eigen::ArrayXf & costs, + const std::string & ns, + const builtin_interfaces::msg::Time & cmd_stamp); /** * @brief Create footprint markers from trajectory @@ -117,18 +154,13 @@ class TrajectoryVisualizer const std_msgs::msg::Header & header, std::shared_ptr costmap_ros); - /** - * @brief Reset object - */ - void reset(); - -protected: std::string frame_id_; nav2::Publisher::SharedPtr trajectories_publisher_; nav2::Publisher::SharedPtr transformed_path_pub_; nav2::Publisher::SharedPtr optimal_path_pub_; nav2::Publisher::SharedPtr optimal_footprints_pub_; + nav2::Publisher::SharedPtr optimal_trajectory_msg_pub_; std::unique_ptr optimal_path_; std::unique_ptr points_; @@ -142,6 +174,10 @@ class TrajectoryVisualizer bool publish_trajectories_with_total_cost_{false}; bool publish_trajectories_with_individual_cost_{false}; bool publish_optimal_footprints_{false}; + bool publish_optimal_trajectory_msg_{false}; + bool publish_transformed_path_{false}; + bool publish_optimal_path_{false}; + int footprint_downsample_factor_{3}; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; }; diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 6707fe222df..96ebc74ed8d 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -37,10 +37,6 @@ void MPPIController::configure( // Get high-level controller parameters auto getParam = parameters_handler_->getParamGetter(name_); - // Get visualization parameters - auto getVisParam = parameters_handler_->getParamGetter(name_ + ".Visualization"); - getVisParam(publish_optimal_trajectory_msg_, "publish_optimal_trajectory_msg", false); - // Configure composed objects optimizer_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get()); @@ -48,11 +44,6 @@ void MPPIController::configure( parent_, name_, costmap_ros_->getGlobalFrameID(), parameters_handler_.get()); - if (publish_optimal_trajectory_msg_) { - opt_traj_pub_ = node->create_publisher( - "~/optimal_trajectory"); - } - RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str()); } @@ -61,7 +52,6 @@ void MPPIController::cleanup() optimizer_.shutdown(); trajectory_visualizer_.on_cleanup(); parameters_handler_.reset(); - opt_traj_pub_.reset(); RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str()); } @@ -70,18 +60,12 @@ void MPPIController::activate() auto node = parent_.lock(); trajectory_visualizer_.on_activate(); parameters_handler_->start(); - if (opt_traj_pub_) { - opt_traj_pub_->on_activate(); - } RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str()); } void MPPIController::deactivate() { trajectory_visualizer_.on_deactivate(); - if (opt_traj_pub_) { - opt_traj_pub_->on_deactivate(); - } RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str()); } @@ -95,9 +79,7 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker) { -#ifdef BENCHMARK_TESTING - auto start = std::chrono::system_clock::now(); -#endif + auto start = std::chrono::steady_clock::now(); std::lock_guard param_lock(*parameters_handler_->getLock()); geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose; @@ -110,43 +92,37 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( auto [cmd, optimal_trajectory] = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); -#ifdef BENCHMARK_TESTING - auto end = std::chrono::system_clock::now(); - auto duration = std::chrono::duration_cast(end - start).count(); - RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); -#endif - - if (publish_optimal_trajectory_msg_ && opt_traj_pub_->get_subscription_count() > 0) { - std_msgs::msg::Header trajectory_header; - trajectory_header.stamp = cmd.header.stamp; - trajectory_header.frame_id = costmap_ros_->getGlobalFrameID(); - auto trajectory_msg = utils::toTrajectoryMsg( - optimal_trajectory, - optimizer_.getOptimalControlSequence(), - optimizer_.getSettings().model_dt, - trajectory_header); - opt_traj_pub_->publish(std::move(trajectory_msg)); - } - - visualize(std::move(transformed_plan), cmd.header, optimal_trajectory); - - return cmd; -} - -void MPPIController::visualize( - nav_msgs::msg::Path transformed_plan, - const std_msgs::msg::Header & header, - const Eigen::ArrayXXf & optimal_trajectory) -{ - // Visualize candidate trajectories (with per-critic costs if available) - trajectory_visualizer_.add( + auto computation_end = std::chrono::steady_clock::now(); + auto computation_time = std::chrono::duration_cast( + computation_end - start).count(); + + // Visualize everything in one consolidated call + trajectory_visualizer_.visualize( + std::move(transformed_plan), + optimal_trajectory, + optimizer_.getOptimalControlSequence(), + optimizer_.getSettings().model_dt, + cmd.header.stamp, + costmap_ros_, optimizer_.getGeneratedTrajectories(), optimizer_.getCosts(), - optimizer_.getCriticCosts(), - header.stamp); + optimizer_.getCriticCosts()); + + auto visualization_end = std::chrono::steady_clock::now(); + auto visualization_time = std::chrono::duration_cast( + visualization_end - computation_end).count(); + auto total_time = std::chrono::duration_cast( + visualization_end - start).count(); + + // Throttled info message every 5 seconds + RCLCPP_INFO_THROTTLE( + logger_, *parent_.lock()->get_clock(), 1000, + "Control loop timing - Computation: %ld μs, Visualization: %ld μs (%.1f%% of total %ld μs)", + computation_time, visualization_time, + (total_time > 0 ? (100.0 * visualization_time / total_time) : 0.0), + total_time); - trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", header.stamp); - trajectory_visualizer_.visualize(std::move(transformed_plan), optimal_trajectory, header, costmap_ros_); + return cmd; } void MPPIController::setPlan(const nav_msgs::msg::Path & path) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index b43a60a6add..7c5aad80d45 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -27,21 +27,39 @@ void TrajectoryVisualizer::on_configure( auto node = parent.lock(); logger_ = node->get_logger(); frame_id_ = frame_id; - trajectories_publisher_ = - node->create_publisher("~/candidate_trajectories"); - transformed_path_pub_ = node->create_publisher( - "~/transformed_global_plan"); - optimal_path_pub_ = node->create_publisher("~/optimal_path"); parameters_handler_ = parameters_handler; - auto getParam = parameters_handler->getParamGetter(name + ".Visualization"); - getParam(trajectory_step_, "trajectory_step", 5); getParam(time_step_, "time_step", 3); getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", false); + getParam(publish_optimal_footprints_, "publish_optimal_footprints", false); + getParam(publish_optimal_trajectory_msg_, "publish_optimal_trajectory_msg", false); + getParam(publish_transformed_path_, "publish_transformed_path", false); + getParam(publish_optimal_path_, "publish_optimal_path", false); + getParam(footprint_downsample_factor_, "footprint_downsample_factor", 3); + + if (publish_trajectories_with_total_cost_ || publish_trajectories_with_individual_cost_) { + trajectories_publisher_ = + node->create_publisher("~/candidate_trajectories"); + } + if (publish_transformed_path_) { + transformed_path_pub_ = node->create_publisher( + "~/transformed_global_plan"); + } + if (publish_optimal_path_) { + optimal_path_pub_ = node->create_publisher("~/optimal_path"); + } + if (publish_optimal_footprints_) { + optimal_footprints_pub_ = node->create_publisher( + "~/optimal_footprints"); + } + if (publish_optimal_trajectory_msg_) { + optimal_trajectory_msg_pub_ = node->create_publisher( + "~/optimal_trajectory"); + } reset(); } @@ -52,26 +70,45 @@ void TrajectoryVisualizer::on_cleanup() transformed_path_pub_.reset(); optimal_path_pub_.reset(); optimal_footprints_pub_.reset(); + optimal_trajectory_msg_pub_.reset(); } void TrajectoryVisualizer::on_activate() { - trajectories_publisher_->on_activate(); - transformed_path_pub_->on_activate(); - optimal_path_pub_->on_activate(); + if (trajectories_publisher_) { + trajectories_publisher_->on_activate(); + } + if (transformed_path_pub_) { + transformed_path_pub_->on_activate(); + } + if (optimal_path_pub_) { + optimal_path_pub_->on_activate(); + } if (optimal_footprints_pub_) { optimal_footprints_pub_->on_activate(); } + if (optimal_trajectory_msg_pub_) { + optimal_trajectory_msg_pub_->on_activate(); + } } void TrajectoryVisualizer::on_deactivate() { - trajectories_publisher_->on_deactivate(); - transformed_path_pub_->on_deactivate(); - optimal_path_pub_->on_deactivate(); + if (trajectories_publisher_) { + trajectories_publisher_->on_deactivate(); + } + if (transformed_path_pub_) { + transformed_path_pub_->on_deactivate(); + } + if (optimal_path_pub_) { + optimal_path_pub_->on_deactivate(); + } if (optimal_footprints_pub_) { optimal_footprints_pub_->on_deactivate(); } + if (optimal_trajectory_msg_pub_) { + optimal_trajectory_msg_pub_->on_deactivate(); + } } void TrajectoryVisualizer::add( @@ -125,117 +162,103 @@ void TrajectoryVisualizer::add( // Check if we should visualize per-critic costs bool visualize_per_critic = !individual_critics_cost.empty() && publish_trajectories_with_individual_cost_ && - trajectories_publisher_->get_subscription_count() > 0; + trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0; size_t n_rows = trajectories.x.rows(); - size_t n_cols = trajectories.x.cols(); points_->markers.reserve(n_rows / trajectory_step_); - // Helper lambda to create a trajectory marker with given costs and namespace - auto create_trajectory_markers = [&]( - const Eigen::ArrayXf & costs, - const std::string & ns, - bool use_collision_coloring) { - - // Find min/max cost for normalization - float min_cost = std::numeric_limits::max(); - float max_cost = std::numeric_limits::lowest(); - - for (Eigen::Index i = 0; i < costs.size(); ++i) { - // Skip collision trajectories for min/max calculation if using collision coloring - if (use_collision_coloring && costs(i) >= 1000000.0f) { - continue; - } - min_cost = std::min(min_cost, costs(i)); - max_cost = std::max(max_cost, costs(i)); - } + // Visualize total costs if requested or if not doing per-critic visualization + if (publish_trajectories_with_total_cost_ || !visualize_per_critic) { + createTrajectoryMarkers(trajectories, total_costs, "Total Costs", cmd_stamp); + } + + // Visualize each critic's contribution if requested + if (visualize_per_critic) { + for (const auto & [critic_name, costs] : individual_critics_cost) { + createTrajectoryMarkers(trajectories, costs, critic_name, cmd_stamp); + } + } +} - float cost_range = max_cost - min_cost; +void TrajectoryVisualizer::createTrajectoryMarkers( + const models::Trajectories & trajectories, + const Eigen::ArrayXf & costs, + const std::string & ns, + const builtin_interfaces::msg::Time & cmd_stamp) +{ + size_t n_rows = trajectories.x.rows(); + size_t n_cols = trajectories.x.cols(); - // Avoid division by zero - if (cost_range < 1e-6f) { - cost_range = 1.0f; - } + // Find min/max cost for normalization + float min_cost = std::numeric_limits::max(); + float max_cost = std::numeric_limits::lowest(); - for (size_t i = 0; i < n_rows; i += trajectory_step_) { - float red_component, green_component, blue_component; - - // Check if this trajectory is in collision (cost >= 1000000) - bool in_collision = use_collision_coloring && costs(i) >= 1000000.0f; - - // Check if cost is zero (no contribution from this critic) - bool zero_cost = std::abs(costs(i)) < 1e-6f; - - if (in_collision) { - // Fixed red color for collision trajectories - red_component = 1.0f; - green_component = 0.0f; - blue_component = 0.0f; - } else if (zero_cost) { - // Gray color for zero cost (no contribution) - red_component = 0.5f; - green_component = 0.5f; - blue_component = 0.5f; - } else { - // Normal gradient for non-collision trajectories - float normalized_cost = (costs(i) - min_cost) / cost_range; - normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f); - - // Color scheme: Green (low cost) -> Yellow -> Red (high cost) - blue_component = 0.0f; - if (normalized_cost < 0.5f) { - // Green to Yellow (0.0 - 0.5) - red_component = normalized_cost * 2.0f; - green_component = 1.0f; - } else { - // Yellow to Red (0.5 - 1.0) - red_component = 1.0f; - green_component = 2.0f * (1.0f - normalized_cost); - } - } - - // Create line strip marker for this trajectory - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame_id_; - marker.header.stamp = cmd_stamp; - marker.ns = ns; - marker.id = marker_id_++; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.01; // Line width - marker.color.r = red_component; - marker.color.g = green_component; - marker.color.b = blue_component; - marker.color.a = 1.0; - - // Add all points in this trajectory to the line strip - for (size_t j = 0; j < n_cols; j += time_step_) { - geometry_msgs::msg::Point point; - point.x = trajectories.x(i, j); - point.y = trajectories.y(i, j); - point.z = 0.0f; - marker.points.push_back(point); - } - - points_->markers.push_back(marker); - } - }; + for (Eigen::Index i = 0; i < costs.size(); ++i) { + min_cost = std::min(min_cost, costs(i)); + max_cost = std::max(max_cost, costs(i)); + } - // If visualizing per-critic costs - if (visualize_per_critic) { - // Visualize total costs if requested - if (publish_trajectories_with_total_cost_) { - create_trajectory_markers(total_costs, "Total Costs", false); + float cost_range = max_cost - min_cost; + + // Avoid division by zero + if (cost_range < 1e-6f) { + cost_range = 1.0f; + } + + for (size_t i = 0; i < n_rows; i += trajectory_step_) { + float red_component, green_component, blue_component; + + // Check if cost is zero (no contribution from this critic) + bool zero_cost = std::abs(costs(i)) < 1e-6f; + + if (zero_cost) { + // Gray color for zero cost (no contribution) + red_component = 0.5f; + green_component = 0.5f; + blue_component = 0.5f; + } else { + // Normal gradient for trajectories + float normalized_cost = (costs(i) - min_cost) / cost_range; + normalized_cost = std::clamp(normalized_cost, 0.0f, 1.0f); + + // Color scheme: Green (low cost) -> Yellow -> Red (high cost) + blue_component = 0.0f; + if (normalized_cost < 0.5f) { + // Green to Yellow (0.0 - 0.5) + red_component = normalized_cost * 2.0f; + green_component = 1.0f; + } else { + // Yellow to Red (0.5 - 1.0) + red_component = 1.0f; + green_component = 2.0f * (1.0f - normalized_cost); + } } - // Visualize each critic's contribution - for (const auto & [critic_name, costs] : individual_critics_cost) { - create_trajectory_markers(costs, critic_name, true); // Use collision coloring + // Create line strip marker for this trajectory + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame_id_; + marker.header.stamp = cmd_stamp; + marker.ns = ns; + marker.id = marker_id_++; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; // Line width + marker.color.r = red_component; + marker.color.g = green_component; + marker.color.b = blue_component; + marker.color.a = 1.0; + + // Add all points in this trajectory to the line strip + for (size_t j = 0; j < n_cols; j += time_step_) { + geometry_msgs::msg::Point point; + point.x = trajectories.x(i, j); + point.y = trajectories.y(i, j); + point.z = 0.0f; + marker.points.push_back(point); } - } else { - // Simple visualization with just total costs - create_trajectory_markers(total_costs, "Total Costs", false); + + points_->markers.push_back(marker); } } @@ -247,31 +270,91 @@ void TrajectoryVisualizer::reset() } void TrajectoryVisualizer::visualize( - const nav_msgs::msg::Path & plan, + nav_msgs::msg::Path plan, const Eigen::ArrayXXf & optimal_trajectory, - const std_msgs::msg::Header & header, - std::shared_ptr costmap_ros) + const models::ControlSequence & control_sequence, + float model_dt, + const builtin_interfaces::msg::Time & stamp, + std::shared_ptr costmap_ros, + const models::Trajectories & candidate_trajectories, + const Eigen::ArrayXf & costs, + const std::vector> & critic_costs) { - if (trajectories_publisher_->get_subscription_count() > 0) { + // Create header with frame from costmap + std_msgs::msg::Header header; + header.stamp = stamp; + header.frame_id = costmap_ros->getGlobalFrameID(); + + // Visualize trajectories with total costs + if (publish_trajectories_with_total_cost_ || + (!publish_trajectories_with_individual_cost_ || critic_costs.empty())) + { + add(candidate_trajectories, costs, {}, stamp); + } + + // Visualize trajectories with individual critic costs + if (publish_trajectories_with_individual_cost_ && !critic_costs.empty()) { + add(candidate_trajectories, costs, critic_costs, stamp); + } + + // Publish trajectories + if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) { trajectories_publisher_->publish(std::move(points_)); } - if (publish_optimal_trajectory_ && optimal_path_pub_->get_subscription_count() > 0) { + // Publish optimal path if enabled + if (publish_optimal_trajectory_ && optimal_path_pub_ && + optimal_path_pub_->get_subscription_count() > 0) + { optimal_path_pub_->publish(std::move(optimal_path_)); } // Publish optimal footprints if enabled if (publish_optimal_footprints_ && optimal_footprints_pub_ && - optimal_footprints_pub_->get_subscription_count() > 0 && - costmap_ros != nullptr && optimal_trajectory.rows() > 0) + optimal_footprints_pub_->get_subscription_count() > 0 && + costmap_ros != nullptr && optimal_trajectory.rows() > 0) { auto footprints_msg = createFootprintMarkers(optimal_trajectory, header, costmap_ros); optimal_footprints_pub_->publish(std::move(footprints_msg)); } + // Publish optimal trajectory message if enabled + if (publish_optimal_trajectory_msg_ && optimal_trajectory_msg_pub_ && + optimal_trajectory_msg_pub_->get_subscription_count() > 0) + { + auto trajectory_msg = utils::toTrajectoryMsg( + optimal_trajectory, + control_sequence, + model_dt, + header); + optimal_trajectory_msg_pub_->publish(std::move(trajectory_msg)); + } + + reset(); + + // Publish transformed path + if (transformed_path_pub_ && transformed_path_pub_->get_subscription_count() > 0) { + auto plan_ptr = std::make_unique(plan); + transformed_path_pub_->publish(std::move(plan_ptr)); + } +} + +void TrajectoryVisualizer::visualize(nav_msgs::msg::Path plan) +{ + // Simplified version for testing that only publishes what's been added + if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) { + trajectories_publisher_->publish(std::move(points_)); + } + + if (publish_optimal_trajectory_ && optimal_path_pub_ && + optimal_path_pub_->get_subscription_count() > 0) + { + optimal_path_pub_->publish(std::move(optimal_path_)); + } + reset(); - if (transformed_path_pub_->get_subscription_count() > 0) { + if (transformed_path_pub_ && transformed_path_pub_->get_subscription_count() > 0) { auto plan_ptr = std::make_unique(plan); transformed_path_pub_->publish(std::move(plan_ptr)); } @@ -283,38 +366,34 @@ visualization_msgs::msg::MarkerArray TrajectoryVisualizer::createFootprintMarker std::shared_ptr costmap_ros) { visualization_msgs::msg::MarkerArray marker_array; - if (trajectory.rows() == 0) { return marker_array; } - // Get robot footprint from costmap - auto robot_footprint = costmap_ros->getRobotFootprint(); - + // Get robot footprint + std::vector robot_footprint = + costmap_ros->getRobotFootprint(); + // Skip if footprint is empty or very small if (robot_footprint.size() < 3) { return marker_array; } - // Create header with costmap frame + // Create header for markers std_msgs::msg::Header costmap_header; - costmap_header.stamp = header.stamp; costmap_header.frame_id = costmap_ros->getGlobalFrameID(); + costmap_header.stamp = header.stamp; - // Sample every N points to avoid too many markers (adjust as needed) - const int footprint_downsample_factor = std::max(1, static_cast(trajectory.rows() / 20)); - int marker_id = 0; - for (int i = 0; i < trajectory.rows(); i += footprint_downsample_factor) { + for (int i = 0; i < trajectory.rows(); i += footprint_downsample_factor_) { double x = trajectory(i, 0); double y = trajectory(i, 1); double theta = trajectory(i, 2); - + // Create oriented footprint geometry_msgs::msg::PolygonStamped oriented_footprint; oriented_footprint.header = costmap_header; nav2_costmap_2d::transformFootprint(x, y, theta, robot_footprint, oriented_footprint); - // Create marker for this footprint visualization_msgs::msg::Marker marker; marker.header = costmap_header; @@ -323,14 +402,12 @@ visualization_msgs::msg::MarkerArray TrajectoryVisualizer::createFootprintMarker marker.type = visualization_msgs::msg::Marker::LINE_STRIP; marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.orientation.w = 1.0; - // Set marker scale and color marker.scale.x = 0.02; // Line width marker.color.r = 0.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.color.a = 0.8; - // Add footprint points to marker for (const auto & point : oriented_footprint.polygon.points) { geometry_msgs::msg::Point p; @@ -339,15 +416,14 @@ visualization_msgs::msg::MarkerArray TrajectoryVisualizer::createFootprintMarker p.z = 0.0; marker.points.push_back(p); } - // Close the polygon by adding the first point again if (!marker.points.empty()) { marker.points.push_back(marker.points[0]); } - + marker_array.markers.push_back(marker); } - + return marker_array; } From 1890dd92a66a1ea0e7e0a5bc89b6652bc8864b0f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 15:23:39 +0100 Subject: [PATCH 11/19] . Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/trajectory_visualizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 7c5aad80d45..ab1e836e298 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -167,8 +167,8 @@ void TrajectoryVisualizer::add( size_t n_rows = trajectories.x.rows(); points_->markers.reserve(n_rows / trajectory_step_); - // Visualize total costs if requested or if not doing per-critic visualization - if (publish_trajectories_with_total_cost_ || !visualize_per_critic) { + // Visualize total costs if requested + if (publish_trajectories_with_total_cost_) { createTrajectoryMarkers(trajectories, total_costs, "Total Costs", cmd_stamp); } From b400b90eb9a3a4eaa5603e4edeb341a3655523df Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 15:32:47 +0100 Subject: [PATCH 12/19] lint Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/critic_data.hpp | 2 ++ nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp | 2 ++ .../nav2_mppi_controller/tools/trajectory_visualizer.hpp | 2 ++ nav2_mppi_controller/src/critics/path_align_critic.cpp | 1 - nav2_mppi_controller/src/critics/path_angle_critic.cpp | 1 - nav2_mppi_controller/src/critics/path_follow_critic.cpp | 1 - 6 files changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp index c40eb7cd04e..542f68a1fd4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include "geometry_msgs/msg/pose_stamped.hpp" diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 140da9eddfd..6fd16979f17 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include "rclcpp_lifecycle/lifecycle_node.hpp" 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 f0e027da2b2..4b0e53c3511 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 @@ -19,6 +19,8 @@ #include #include +#include +#include #include "nav_msgs/msg/path.hpp" #include "nav2_msgs/msg/trajectory.hpp" diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index cbf06f1c240..f7dfd556425 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "nav2_mppi_controller/critics/path_align_critic.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" namespace mppi::critics { diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index 9e3f6d7bde3..d1626ca8867 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -14,7 +14,6 @@ // limitations under the License. #include "nav2_mppi_controller/critics/path_angle_critic.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 9eb45625ff1..66ea0a4b387 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "nav2_mppi_controller/critics/path_follow_critic.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include From a3611d47f378b982e77ddd60eac12ae562e074cd Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 15:40:41 +0100 Subject: [PATCH 13/19] rename visualize Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/critics/path_align_critic.hpp | 2 +- .../nav2_mppi_controller/critics/path_angle_critic.hpp | 2 +- .../nav2_mppi_controller/critics/path_follow_critic.hpp | 2 +- nav2_mppi_controller/src/critics/path_align_critic.cpp | 6 +++--- nav2_mppi_controller/src/critics/path_angle_critic.cpp | 6 +++--- nav2_mppi_controller/src/critics/path_follow_critic.cpp | 6 +++--- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 7b194c5667b..6cc33a509a5 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -56,7 +56,7 @@ class PathAlignCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool visualize_{false}; + bool visualize_furthest_point_{false}; nav2::Publisher::SharedPtr furthest_point_pub_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp index 98cfa1289fd..e127abb3ff8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -85,7 +85,7 @@ class PathAngleCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool visualize_{false}; + bool visualize_furthest_point_{false}; nav2::Publisher::SharedPtr furthest_point_pub_; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp index 539ea798a2e..c38fcd52b62 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp @@ -57,7 +57,7 @@ class PathFollowCritic : public CriticFunction unsigned int power_{0}; float weight_{0}; - bool visualize_{false}; + bool visualize_furthest_point_{false}; nav2::Publisher::SharedPtr furthest_point_pub_; }; diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index f7dfd556425..37076da3b3d 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -30,9 +30,9 @@ void PathAlignCritic::initialize() threshold_to_consider_, "threshold_to_consider", 0.5f); getParam(use_path_orientations_, "use_path_orientations", false); - getParam(visualize_, "visualize", false); + getParam(visualize_furthest_point_, "visualize_furthest_point", false); - if (visualize_) { + if (visualize_furthest_point_) { auto node = parent_.lock(); if (node) { furthest_point_pub_ = node->create_publisher( @@ -60,7 +60,7 @@ void PathAlignCritic::score(CriticData & data) float path_segments_flt = static_cast(path_segments_count); // Visualize target pose if enabled - if (visualize_ && path_segments_count > 0 && + if (visualize_furthest_point_ && path_segments_count > 0 && furthest_point_pub_->get_subscription_count() > 0) { auto furthest_point = std::make_unique(); diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp index d1626ca8867..e2fb16c1d0f 100644 --- a/nav2_mppi_controller/src/critics/path_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -41,7 +41,7 @@ void PathAngleCritic::initialize() getParam( max_angle_to_furthest_, "max_angle_to_furthest", 0.785398f); - getParam(visualize_, "visualize", false); + getParam(visualize_furthest_point_, "visualize_furthest_point", false); int mode = 0; getParam(mode, "mode", mode); @@ -54,7 +54,7 @@ void PathAngleCritic::initialize() "don't allow for reversing! Setting mode to forward preference."); } - if (visualize_) { + if (visualize_furthest_point_) { auto node = parent_.lock(); if (node) { furthest_point_pub_ = node->create_publisher( @@ -86,7 +86,7 @@ void PathAngleCritic::score(CriticData & data) const geometry_msgs::msg::Pose & pose = data.state.pose.pose; // Visualize target pose if enabled - if (visualize_ && furthest_point_pub_->get_subscription_count() > 0) { + if (visualize_furthest_point_ && furthest_point_pub_->get_subscription_count() > 0) { auto furthest_point = std::make_unique(); furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); furthest_point->header.stamp = clock_->now(); diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp index 66ea0a4b387..c7a70cdb22c 100644 --- a/nav2_mppi_controller/src/critics/path_follow_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -29,9 +29,9 @@ void PathFollowCritic::initialize() getParam(offset_from_furthest_, "offset_from_furthest", 6); getParam(power_, "cost_power", 1); getParam(weight_, "cost_weight", 5.0f); - getParam(visualize_, "visualize", false); + getParam(visualize_furthest_point_, "visualize_furthest_point", false); - if (visualize_) { + if (visualize_furthest_point_) { auto node = parent_.lock(); if (node) { furthest_point_pub_ = node->create_publisher( @@ -71,7 +71,7 @@ void PathFollowCritic::score(CriticData & data) const auto path_x = data.path.x(offsetted_idx); const auto path_y = data.path.y(offsetted_idx); // Visualize target pose if enabled - if (visualize_ && furthest_point_pub_->get_subscription_count() > 0) { + if (visualize_furthest_point_ && furthest_point_pub_->get_subscription_count() > 0) { auto furthest_point = std::make_unique(); furthest_point->header.frame_id = costmap_ros_->getGlobalFrameID(); furthest_point->header.stamp = clock_->now(); From 41b4c35d824bdff81f034bb324e35f22096b03b0 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 15:53:36 +0100 Subject: [PATCH 14/19] small fixes Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/controller.cpp | 28 +++++++-------------- nav2_mppi_controller/src/critic_manager.cpp | 2 +- 2 files changed, 10 insertions(+), 20 deletions(-) diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp index 96ebc74ed8d..f7ce3f4e549 100644 --- a/nav2_mppi_controller/src/controller.cpp +++ b/nav2_mppi_controller/src/controller.cpp @@ -79,7 +79,9 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( const geometry_msgs::msg::Twist & robot_speed, nav2_core::GoalChecker * goal_checker) { - auto start = std::chrono::steady_clock::now(); + #ifdef BENCHMARK_TESTING + auto start = std::chrono::system_clock::now(); + #endif std::lock_guard param_lock(*parameters_handler_->getLock()); geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose; @@ -92,11 +94,13 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( auto [cmd, optimal_trajectory] = optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker); - auto computation_end = std::chrono::steady_clock::now(); - auto computation_time = std::chrono::duration_cast( - computation_end - start).count(); - // Visualize everything in one consolidated call + #ifdef BENCHMARK_TESTING + auto end = std::chrono::system_clock::now(); + auto duration = std::chrono::duration_cast(end - start).count(); + RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration); + #endif + trajectory_visualizer_.visualize( std::move(transformed_plan), optimal_trajectory, @@ -108,20 +112,6 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands( optimizer_.getCosts(), optimizer_.getCriticCosts()); - auto visualization_end = std::chrono::steady_clock::now(); - auto visualization_time = std::chrono::duration_cast( - visualization_end - computation_end).count(); - auto total_time = std::chrono::duration_cast( - visualization_end - start).count(); - - // Throttled info message every 5 seconds - RCLCPP_INFO_THROTTLE( - logger_, *parent_.lock()->get_clock(), 1000, - "Control loop timing - Computation: %ld μs, Visualization: %ld μs (%.1f%% of total %ld μs)", - computation_time, visualization_time, - (total_time > 0 ? (100.0 * visualization_time / total_time) : 0.0), - total_time); - return cmd; } diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp index 495f98fafa9..0f72f9f6a47 100644 --- a/nav2_mppi_controller/src/critic_manager.cpp +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -102,7 +102,7 @@ void CriticManager::evalTrajectoriesScores( break; } - // Store costs before critic evaluation (only if needed) + // Store costs before critic evaluation Eigen::ArrayXf costs_before; if (visualize_per_critic_costs_ || publish_critics_stats_) { costs_before = data.costs; From 0f57a9c5b72645223d93f6eeb321da12db840bad Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 16:57:13 +0100 Subject: [PATCH 15/19] add optimal traj Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/trajectory_visualizer.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index ab1e836e298..2743b181445 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -297,6 +297,11 @@ void TrajectoryVisualizer::visualize( add(candidate_trajectories, costs, critic_costs, stamp); } + // Add optimal trajectory to populate optimal_path_ + if (publish_optimal_trajectory_ && optimal_trajectory.rows() > 0) { + add(optimal_trajectory, "Optimal Trajectory", stamp); + } + // Publish trajectories if (trajectories_publisher_ && trajectories_publisher_->get_subscription_count() > 0) { trajectories_publisher_->publish(std::move(points_)); From 70e71620e8a20118e8cd27cb178acfc153ef2815 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 17:07:06 +0100 Subject: [PATCH 16/19] updated params Signed-off-by: Tony Najjar --- nav2_bringup/params/nav2_params.yaml | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index e6521258ffc..efa1828f9a8 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -138,12 +138,19 @@ controller_server: temperature: 0.3 gamma: 0.015 motion_model: "DiffDrive" - visualize: true - publish_optimal_trajectory: true regenerate_noises: true - TrajectoryVisualizer: + Visualization: + publish_critics_stats: true + publish_optimal_trajectory_msg: true + publish_optimal_trajectory: true + publish_transformed_path: true + publish_trajectories_with_total_cost: true + publish_trajectories_with_individual_cost: true + publish_optimal_footprints: true + publish_optimal_path: true trajectory_step: 5 time_step: 3 + footprint_downsample_factor: 3 TrajectoryValidator: # The validator can be configured with additional parameters if needed. plugin: "mppi::DefaultOptimalTrajectoryValidator" From a88f5dc1db7e2687d0741ce3e01c6975e757fbd3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Oct 2025 17:07:26 +0100 Subject: [PATCH 17/19] set params to false Signed-off-by: Tony Najjar --- nav2_bringup/params/nav2_params.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index efa1828f9a8..da8c17d1cfe 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -140,14 +140,14 @@ controller_server: motion_model: "DiffDrive" regenerate_noises: true Visualization: - publish_critics_stats: true - publish_optimal_trajectory_msg: true - publish_optimal_trajectory: true - publish_transformed_path: true - publish_trajectories_with_total_cost: true - publish_trajectories_with_individual_cost: true - publish_optimal_footprints: true - publish_optimal_path: true + publish_critics_stats: false + publish_optimal_trajectory_msg: false + publish_optimal_trajectory: false + publish_transformed_path: false + publish_trajectories_with_total_cost: false + publish_trajectories_with_individual_cost: false + publish_optimal_footprints: false + publish_optimal_path: false trajectory_step: 5 time_step: 3 footprint_downsample_factor: 3 From badadf1ac1cc90a2b4453cec26888ae7f4ddddca Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 10 Nov 2025 15:21:55 +0100 Subject: [PATCH 18/19] fix unused var Signed-off-by: Tony Najjar --- .../nav2_mppi_controller/tools/trajectory_visualizer.hpp | 1 - nav2_mppi_controller/src/trajectory_visualizer.cpp | 7 +++---- 2 files changed, 3 insertions(+), 5 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 4b0e53c3511..858a67d5efe 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 @@ -172,7 +172,6 @@ class TrajectoryVisualizer size_t trajectory_step_{0}; size_t time_step_{0}; - bool publish_optimal_trajectory_{false}; bool publish_trajectories_with_total_cost_{false}; bool publish_trajectories_with_individual_cost_{false}; bool publish_optimal_footprints_{false}; diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp index 2743b181445..462a9340e51 100644 --- a/nav2_mppi_controller/src/trajectory_visualizer.cpp +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -31,7 +31,6 @@ void TrajectoryVisualizer::on_configure( auto getParam = parameters_handler->getParamGetter(name + ".Visualization"); getParam(trajectory_step_, "trajectory_step", 5); getParam(time_step_, "time_step", 3); - getParam(publish_optimal_trajectory_, "publish_optimal_trajectory", false); getParam(publish_trajectories_with_total_cost_, "publish_trajectories_with_total_cost", false); getParam(publish_trajectories_with_individual_cost_, "publish_trajectories_with_individual_cost", false); @@ -298,7 +297,7 @@ void TrajectoryVisualizer::visualize( } // Add optimal trajectory to populate optimal_path_ - if (publish_optimal_trajectory_ && optimal_trajectory.rows() > 0) { + if (publish_optimal_path_ && optimal_trajectory.rows() > 0) { add(optimal_trajectory, "Optimal Trajectory", stamp); } @@ -308,7 +307,7 @@ void TrajectoryVisualizer::visualize( } // Publish optimal path if enabled - if (publish_optimal_trajectory_ && optimal_path_pub_ && + if (publish_optimal_path_ && optimal_path_pub_ && optimal_path_pub_->get_subscription_count() > 0) { optimal_path_pub_->publish(std::move(optimal_path_)); @@ -351,7 +350,7 @@ void TrajectoryVisualizer::visualize(nav_msgs::msg::Path plan) trajectories_publisher_->publish(std::move(points_)); } - if (publish_optimal_trajectory_ && optimal_path_pub_ && + if (publish_optimal_path_ && optimal_path_pub_ && optimal_path_pub_->get_subscription_count() > 0) { optimal_path_pub_->publish(std::move(optimal_path_)); From 303e0cb1fea54b5b215c0133d2eef6ac6937fdba Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 10 Nov 2025 15:25:30 +0100 Subject: [PATCH 19/19] Update nav2_params.yaml Signed-off-by: Tony Najjar --- nav2_bringup/params/nav2_params.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index da8c17d1cfe..b372f80ce91 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -142,7 +142,6 @@ controller_server: Visualization: publish_critics_stats: false publish_optimal_trajectory_msg: false - publish_optimal_trajectory: false publish_transformed_path: false publish_trajectories_with_total_cost: false publish_trajectories_with_individual_cost: false