Humble enjoy mppi critic pub and critics goal distance fix

nav2_mppi_controller/CMakeLists.txt

@@ -14,6 +14,7 @@ set(XTENSOR_USE_XSIMD 1)
 find_package(ament_cmake REQUIRED)
 find_package(xtensor REQUIRED)
 find_package(xsimd REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
 include_directories(
   include
@@ -33,6 +34,7 @@ set(dependencies_pkgs
   tf2_geometry_msgs
   tf2_eigen
   tf2_ros
+  std_msgs
 )
 
 foreach(pkg IN LISTS dependencies_pkgs)
@@ -41,6 +43,12 @@ endforeach()
 
 nav2_package()
 
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/CriticScore.msg"
+  "msg/CriticScores.msg"
+  DEPENDENCIES std_msgs
+)
+
 include(CheckCXXCompilerFlag)
 
 check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512)
@@ -120,8 +128,14 @@ if(BUILD_TESTING)
   # add_subdirectory(benchmark)
 endif()
 
+rosidl_get_typesupport_target(cpp_typesupport_target
+  ${PROJECT_NAME} rosidl_typesupport_cpp)
+
+target_link_libraries(mppi_controller "${cpp_typesupport_target}")
+
 ament_export_libraries(${libraries})
 ament_export_dependencies(${dependencies_pkgs})
+ament_export_dependencies(rosidl_default_runtime)
 ament_export_include_directories(include)
 pluginlib_export_plugin_description_file(nav2_core mppic.xml)
 pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml)

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -23,6 +23,8 @@
 #include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
 #include "nav2_mppi_controller/models/constraints.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
+#include "nav2_mppi_controller/msg/critic_score.hpp"
+#include "nav2_mppi_controller/msg/critic_scores.hpp"
 
 #include "nav2_core/controller.hpp"
 #include "nav2_core/goal_checker.hpp"
@@ -121,10 +123,14 @@ class MPPIController : public nav2_core::Controller
   TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
+  bool publish_critics_;
 
   double reset_period_;
   // Last time computeVelocityCommands was called
   rclcpp::Time last_time_called_;
+
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav2_mppi_controller::msg::CriticScores>>
+  critics_publisher_;
 };
 
 }  // namespace nav2_mppi_controller

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

@@ -32,14 +32,15 @@ namespace mppi
 
 /**
  * @struct mppi::CriticData
- * @brief Data to pass to critics for scoring, including state, trajectories, path, costs, and
- * important parameters to share
+ * @brief Data to pass to critics for scoring, including state, trajectories,
+ * pruned path, global goal, costs, and important parameters to share
  */
 struct CriticData
 {
   const models::State & state;
   const models::Trajectories & trajectories;
   const models::Path & path;
+  const geometry_msgs::msg::Pose & goal;
 
   xt::xtensor<float, 1> & costs;
   float & model_dt;

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

@@ -46,12 +46,12 @@ class CriticManager
     * @brief Constructor for mppi::CriticManager
     */
   CriticManager() = default;
-  
+
   /**
     * @brief Virtual Destructor for mppi::CriticManager
     */
   virtual ~CriticManager() = default;
-  
+
   /**
     * @brief Configure critic manager on bringup and load plugins
     * @param parent WeakPtr to node
@@ -69,6 +69,10 @@ class CriticManager
     */
   void evalTrajectoriesScores(CriticData & data) const;
 
+  xt::xtensor<float, 1> evalTrajectory(CriticData & data) const;
+
+  std::vector<std::string> getCriticNames() const;
+
 protected:
   /**
     * @brief Get parameters (critics to load)

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -90,7 +90,7 @@ class Optimizer
   geometry_msgs::msg::TwistStamped evalControl(
     const geometry_msgs::msg::PoseStamped & robot_pose,
     const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan,
-    nav2_core::GoalChecker * goal_checker);
+    const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
 
   /**
    * @brief Get the trajectories generated in a cycle for visualization
@@ -104,6 +104,15 @@ class Optimizer
    */
   xt::xtensor<float, 2> getOptimizedTrajectory();
 
+
+  /**
+   * @brief Get the critic costs for given trajectory
+   * @return Names and costs of the critics
+   */
+  xt::xtensor<float, 1> getOptimizationResults();
+
+  std::vector<std::string> getCriticNames() const;
+
   /**
    * @brief Set the maximum speed based on the speed limits callback
    * @param speed_limit Limit of the speed for use
@@ -132,7 +141,8 @@ class Optimizer
   void prepare(
     const geometry_msgs::msg::PoseStamped & robot_pose,
     const geometry_msgs::msg::Twist & robot_speed,
-    const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker);
+    const nav_msgs::msg::Path & plan,
+    const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
 
   /**
    * @brief Obtain the main controller's parameters
@@ -250,10 +260,12 @@ class Optimizer
   std::array<mppi::models::Control, 4> control_history_;
   models::Trajectories generated_trajectories_;
   models::Path path_;
+  geometry_msgs::msg::Pose goal_;
   xt::xtensor<float, 1> costs_;
 
-  CriticData critics_data_ =
-  {state_, generated_trajectories_, path_, costs_, settings_.model_dt, false, nullptr, nullptr,
+  CriticData critics_data_ = {
+    state_, generated_trajectories_, path_, goal_,
+    costs_, 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/path_handler.hpp

@@ -89,6 +89,12 @@ class PathHandler
    */
   nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
 
+  /**
+   * @brief Get the global goal pose transformed to the local frame
+   * @return Transformed goal pose
+   */
+  geometry_msgs::msg::PoseStamped getTransformedGoal();
+
 protected:
   /**
     * @brief Transform a pose to another frame

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -195,27 +195,23 @@ inline models::Path toTensor(const nav_msgs::msg::Path & path)
  * @brief Check if the robot pose is within the Goal Checker's tolerances to goal
  * @param global_checker Pointer to the goal checker
  * @param robot Pose of robot
- * @param path Path to retreive goal pose from
+ * @param goal Goal pose
  * @return bool If robot is within goal checker tolerances to the goal
  */
 inline bool withinPositionGoalTolerance(
   nav2_core::GoalChecker * goal_checker,
   const geometry_msgs::msg::Pose & robot,
-  const models::Path & path)
+  const geometry_msgs::msg::Pose & goal)
 {
-  const auto goal_idx = path.x.shape(0) - 1;
-  const auto goal_x = path.x(goal_idx);
-  const auto goal_y = path.y(goal_idx);
-
   if (goal_checker) {
     geometry_msgs::msg::Pose pose_tolerance;
     geometry_msgs::msg::Twist velocity_tolerance;
     goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
 
     const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;
 
-    auto dx = robot.position.x - goal_x;
-    auto dy = robot.position.y - goal_y;
+    auto dx = robot.position.x - goal.position.x;
+    auto dy = robot.position.y - goal.position.y;
 
     auto dist_sq = dx * dx + dy * dy;
 
@@ -231,24 +227,19 @@ inline bool withinPositionGoalTolerance(
  * @brief Check if the robot pose is within tolerance to the goal
  * @param pose_tolerance Pose tolerance to use
  * @param robot Pose of robot
- * @param path Path to retreive goal pose from
+ * @param goal Goal pose
  * @return bool If robot is within tolerance to the goal
  */
 inline bool withinPositionGoalTolerance(
   float pose_tolerance,
   const geometry_msgs::msg::Pose & robot,
-  const models::Path & path)
+  const geometry_msgs::msg::Pose & goal)
 {
-  const auto goal_idx = path.x.shape(0) - 1;
-  const auto goal_x = path.x(goal_idx);
-  const auto goal_y = path.y(goal_idx);
-
-  const auto pose_tolerance_sq = pose_tolerance * pose_tolerance;
-
-  auto dx = robot.position.x - goal_x;
-  auto dy = robot.position.y - goal_y;
+  const double & dist_sq =
+    std::pow(goal.position.x - robot.position.x, 2) +
+    std::pow(goal.position.y - robot.position.y, 2);
 
-  auto dist_sq = dx * dx + dy * dy;
+  const float pose_tolerance_sq = pose_tolerance * pose_tolerance;
 
   if (dist_sq < pose_tolerance_sq) {
     return true;

nav2_mppi_controller/msg/CriticScore.msg

@@ -0,0 +1,2 @@
+std_msgs/String name
+std_msgs/Float32 score

nav2_mppi_controller/msg/CriticScores.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header    # when msg was sent
+CriticScore[] critic_scores

nav2_mppi_controller/package.xml

@@ -10,6 +10,9 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
 
   <depend>rclcpp</depend>
   <depend>nav2_common</depend>
@@ -33,6 +36,8 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
   <export>
     <build_type>ament_cmake</build_type>
     <nav2_core plugin="${prefix}/mppic.xml" />

nav2_mppi_controller/src/controller.cpp

@@ -39,6 +39,7 @@ void MPPIController::configure(
   // Get high-level controller parameters
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(visualize_, "visualize", false);
+  getParam(publish_critics_, "publish_critics", false);
   getParam(reset_period_, "reset_period", 1.0);
 
   // Configure composed objects
@@ -48,6 +49,11 @@ void MPPIController::configure(
     parent_, name_,
     costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
 
+  if (publish_critics_) {
+    critics_publisher_ = node->create_publisher<nav2_mppi_controller::msg::CriticScores>(
+      "/mppi_critic_scores", 1);
+  }
+
   RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
 }
 
@@ -61,13 +67,19 @@ void MPPIController::cleanup()
 
 void MPPIController::activate()
 {
+  if (publish_critics_) {
+    critics_publisher_->on_activate();
+  }
   trajectory_visualizer_.on_activate();
   parameters_handler_->start();
   RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::deactivate()
 {
+  if (publish_critics_) {
+    critics_publisher_->on_deactivate();
+  }
   trajectory_visualizer_.on_deactivate();
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
@@ -92,13 +104,15 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   last_time_called_ = clock_->now();
 
   std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
+  geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal().pose;
+
   nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
 
   nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
   std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
 
   geometry_msgs::msg::TwistStamped cmd =
-    optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
+    optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
 
 #ifdef BENCHMARK_TESTING
   auto end = std::chrono::system_clock::now();
@@ -110,6 +124,36 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
     visualize(std::move(transformed_plan));
   }
 
+  if (publish_critics_) {
+    std::vector<std::string> critic_names = optimizer_.getCriticNames();
+    xt::xtensor<float, 1> critic_costs = optimizer_.getOptimizationResults();
+
+    // log critic names and costs
+    for (size_t i = 0; i < critic_names.size(); i++) {
+      RCLCPP_DEBUG(logger_, "Critic: %s, Cost: %f", critic_names[i].c_str(), critic_costs[i]);
+    }
+
+    // make msg
+    auto critic_scores_ = std::make_unique<nav2_mppi_controller::msg::CriticScores>();
+    if (critic_names.size() != critic_costs.size()) {
+      RCLCPP_ERROR(
+        logger_,
+        "Critic names %ld and costs %ld size mismatch!",
+        critic_names.size(), critic_costs.size());
+      return cmd;
+    }
+
+    for (size_t i = 0; i < critic_names.size(); i++) {
+      nav2_mppi_controller::msg::CriticScore critic_score;
+      critic_score.name.data = critic_names[i];
+      critic_score.score.data = critic_costs[i];
+      critic_scores_->critic_scores.push_back(critic_score);
+    }
+
+    critic_scores_->header.stamp = clock_->now();
+    critics_publisher_->publish(std::move(critic_scores_));
+  }
+
   return cmd;
 }