MPPI cost-based visualization

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -101,31 +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 cmd_stamp Command stamp
-    * @param optimal_trajectory Optimal trajectory, if already computed
-    */
-  void visualize(
-    nav_msgs::msg::Path transformed_plan,
-    const builtin_interfaces::msg::Time & cmd_stamp,
-    const Eigen::ArrayXXf & optimal_trajectory);
-
   std::string name_;
   nav2::LifecycleNode::WeakPtr parent_;
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  nav2::Publisher<nav2_msgs::msg::Trajectory>::SharedPtr opt_traj_pub_;
-
   std::unique_ptr<ParametersHandler> parameters_handler_;
   Optimizer optimizer_;
   PathHandler path_handler_;
   TrajectoryVisualizer trajectory_visualizer_;
-
-  bool visualize_;
-  bool publish_optimal_trajectory_;
 };
 
 }  // namespace nav2_mppi_controller

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

@@ -18,6 +18,8 @@
 #include <Eigen/Dense>
 
 #include <memory>
+#include <string>
+#include <utility>
 #include <vector>
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -44,6 +46,7 @@ struct CriticData
   const geometry_msgs::msg::Pose & goal;
 
   Eigen::ArrayXf & costs;
+  std::optional<std::vector<std::pair<std::string, Eigen::ArrayXf>>> individual_critics_cost;
   float & model_dt;
 
   bool fail_flag;

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

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

@@ -99,6 +99,7 @@ class CriticManager
 
   nav2::Publisher<nav2_msgs::msg::CriticsStats>::SharedPtr critics_effect_pub_;
   bool publish_critics_stats_;
+  bool visualize_per_critic_costs_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
 };

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -18,6 +18,9 @@
 #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 "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_ros_common/publisher.hpp"
 
 namespace mppi::critics
 {
@@ -52,6 +55,9 @@ class PathAlignCritic : public CriticFunction
   bool use_path_orientations_{false};
   unsigned int power_{0};
   float weight_{0};
+
+  bool visualize_furthest_point_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
 };
 
 }  // namespace mppi::critics

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp

@@ -20,6 +20,9 @@
 #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 "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_ros_common/publisher.hpp"
 
 namespace mppi::critics
 {
@@ -81,6 +84,9 @@ class PathAngleCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
+
+  bool visualize_furthest_point_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
 };
 
 }  // namespace mppi::critics

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp

@@ -19,6 +19,9 @@
 #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 "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "nav2_ros_common/publisher.hpp"
 
 namespace mppi::critics
 {
@@ -53,6 +56,9 @@ class PathFollowCritic : public CriticFunction
 
   unsigned int power_{0};
   float weight_{0};
+
+  bool visualize_furthest_point_{false};
+  nav2::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr furthest_point_pub_;
 };
 
 }  // namespace mppi::critics

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -20,6 +20,8 @@
 #include <string>
 #include <memory>
 #include <tuple>
+#include <utility>
+#include <vector>
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -118,6 +120,28 @@ 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 Get the per-critic costs for trajectories for visualization
+   * @return Vector of (critic_name, costs) pairs
+   */
+  const std::vector<std::pair<std::string, Eigen::ArrayXf>> & getCriticCosts() const
+  {
+    if (critics_data_.individual_critics_cost) {
+      return *critics_data_.individual_critics_cost;
+    }
+    static const std::vector<std::pair<std::string, Eigen::ArrayXf>> empty;
+    return empty;
+  }
+
   /**
    * @brief Set the maximum speed based on the speed limits callback
    * @param speed_limit Limit of the speed for use
@@ -284,7 +308,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")};

nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp

@@ -19,14 +19,20 @@
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 #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"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
 #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
 {
@@ -78,28 +84,85 @@ 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 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 std::string & marker_namespace);
+  void add(
+    const models::Trajectories & trajectories,
+    const Eigen::ArrayXf & total_costs,
+    const std::vector<std::pair<std::string, Eigen::ArrayXf>> & individual_critics_cost = {},
+    const builtin_interfaces::msg::Time & cmd_stamp = builtin_interfaces::msg::Time());
+
+  /**
+    * @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(
+    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<nav2_costmap_2d::Costmap2DROS> costmap_ros,
+    const models::Trajectories & candidate_trajectories,
+    const Eigen::ArrayXf & costs,
+    const std::vector<std::pair<std::string, Eigen::ArrayXf>> & critic_costs);
 
   /**
-    * @brief Visualize the plan
-    * @param plan Plan to visualize
+    * @brief Visualize without optimizer (for testing)
+    * @param plan Transformed plan to visualize
     */
-  void visualize(const nav_msgs::msg::Path & plan);
+  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
+    * @param trajectory Optimal trajectory
+    * @param header Message header
+    * @param costmap_ros Costmap ROS pointer for footprint and frame information
+    * @return MarkerArray containing footprint polygons
+    */
+  visualization_msgs::msg::MarkerArray createFootprintMarkers(
+    const Eigen::ArrayXXf & trajectory,
+    const std_msgs::msg::Header & header,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
+
   std::string frame_id_;
   nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr
     trajectories_publisher_;
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr transformed_path_pub_;
   nav2::Publisher<nav_msgs::msg::Path>::SharedPtr optimal_path_pub_;
+  nav2::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr optimal_footprints_pub_;
+  nav2::Publisher<nav2_msgs::msg::Trajectory>::SharedPtr optimal_trajectory_msg_pub_;
 
   std::unique_ptr<nav_msgs::msg::Path> optimal_path_;
   std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
@@ -109,6 +172,14 @@ 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};
+  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")};
 };