Add goal pose to CriticData

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/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
@@ -132,7 +132,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 +251,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

@@ -229,6 +229,11 @@ inline bool withinPositionGoalTolerance(
 
 /**
  * @brief Check if the robot pose is within tolerance to the goal
+ *
+ * Deprecated!
+ * This uses the end of pruned path as the goal, if you want to use
+ * the global goal, use the other overload
+ *
  * @param pose_tolerance Pose tolerance to use
  * @param robot Pose of robot
  * @param path Path to retreive goal pose from
@@ -257,6 +262,29 @@ inline bool withinPositionGoalTolerance(
   return false;
 }
 
+/**
+ * @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
+ * @return bool If robot is within tolerance to the goal
+ */
+inline bool withinPositionGoalTolerance(
+  float pose_tolerance, const CriticData & data)
+{
+  const double & dist_sq =
+    std::pow(data.goal.position.x - data.state.pose.pose.position.x, 2) +
+    std::pow(data.goal.position.y - data.state.pose.pose.position.y, 2);
+
+  const auto pose_tolerance_sq = pose_tolerance * pose_tolerance;
+
+  if (dist_sq < pose_tolerance_sq) {
+    return true;
+  }
+
+  return false;
+}
+
 /**
   * @brief normalize
   * Normalizes the angle to be -M_PI circle to +M_PI circle

nav2_mppi_controller/src/controller.cpp

@@ -92,13 +92,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();

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -119,7 +119,7 @@ void CostCritic::score(CriticData & data)
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
-  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data)) {
     near_goal = true;
   }
 

nav2_mppi_controller/src/critics/goal_angle_critic.cpp

@@ -35,8 +35,7 @@ void GoalAngleCritic::initialize()
 
 void GoalAngleCritic::score(CriticData & data)
 {
-  if (!enabled_ || !utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.path))
+  if (!enabled_ || !utils::withinPositionGoalTolerance(threshold_to_consider_, data))
   {
     return;
   }

nav2_mppi_controller/src/critics/goal_critic.cpp

@@ -35,16 +35,13 @@ void GoalCritic::initialize()
 
 void GoalCritic::score(CriticData & data)
 {
-  if (!enabled_ || !utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.path))
+  if (!enabled_ || !utils::withinPositionGoalTolerance(threshold_to_consider_, data))
   {
     return;
   }
 
-  const auto goal_idx = data.path.x.shape(0) - 1;
-
-  const auto goal_x = data.path.x(goal_idx);
-  const auto goal_y = data.path.y(goal_idx);
+  const auto & goal_x = data.goal.position.x;
+  const auto & goal_y = data.goal.position.y;
 
   const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
   const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -125,7 +125,7 @@ void ObstaclesCritic::score(CriticData & data)
 
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
-  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data)) {
     near_goal = true;
   }
 

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -46,8 +46,7 @@ void PathAlignCritic::initialize()
 void PathAlignCritic::score(CriticData & data)
 {
   // Don't apply close to goal, let the goal critics take over
-  if (!enabled_ ||
-    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
+  if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data))
   {
     return;
   }

nav2_mppi_controller/src/critics/path_align_legacy_critic.cpp

@@ -46,8 +46,7 @@ void PathAlignLegacyCritic::initialize()
 void PathAlignLegacyCritic::score(CriticData & data)
 {
   // Don't apply close to goal, let the goal critics take over
-  if (!enabled_ ||
-    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
+  if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data))
   {
     return;
   }

nav2_mppi_controller/src/critics/path_angle_critic.cpp

@@ -62,7 +62,7 @@ void PathAngleCritic::score(CriticData & data)
     return;
   }
 
-  if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) {
+  if (utils::withinPositionGoalTolerance(threshold_to_consider_, data)) {
     return;
   }
 

nav2_mppi_controller/src/critics/path_follow_critic.cpp

@@ -35,7 +35,7 @@ void PathFollowCritic::initialize()
 void PathFollowCritic::score(CriticData & data)
 {
   if (!enabled_ || data.path.x.shape(0) < 2 ||
-    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
+    utils::withinPositionGoalTolerance(threshold_to_consider_, data))
   {
     return;
   }

nav2_mppi_controller/src/critics/prefer_forward_critic.cpp

@@ -33,8 +33,7 @@ void PreferForwardCritic::initialize()
 void PreferForwardCritic::score(CriticData & data)
 {
   using xt::evaluation_strategy::immediate;
-  if (!enabled_ ||
-    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
+  if (!enabled_ || utils::withinPositionGoalTolerance(threshold_to_consider_, data))
   {
     return;
   }

nav2_mppi_controller/src/optimizer.cpp

@@ -134,9 +134,11 @@ void Optimizer::reset()
 geometry_msgs::msg::TwistStamped Optimizer::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 nav_msgs::msg::Path & plan,
+  const geometry_msgs::msg::Pose & goal,
+  nav2_core::GoalChecker * goal_checker)
 {
-  prepare(robot_pose, robot_speed, plan, goal_checker);
+  prepare(robot_pose, robot_speed, plan, goal, goal_checker);
 
   do {
     optimize();
@@ -183,11 +185,15 @@ bool Optimizer::fallback(bool fail)
 void Optimizer::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)
 {
   state_.pose = robot_pose;
   state_.speed = robot_speed;
   path_ = utils::toTensor(plan);
+  goal_ = goal;
+
   costs_.fill(0);
 
   critics_data_.fail_flag = false;

nav2_mppi_controller/src/path_handler.cpp

@@ -186,6 +186,20 @@ void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
   plan.poses.erase(plan.poses.begin(), end);
 }
 
+geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal()
+{
+  auto goal = global_plan_.poses.back();
+  goal.header.stamp = rclcpp::Time(0);
+  if (goal.header.frame_id.empty()) {
+    throw std::runtime_error("Goal pose has an empty frame_id");
+  }
+  geometry_msgs::msg::PoseStamped transformed_goal;
+  if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) {
+    throw std::runtime_error("Unable to transform goal pose into costmap frame");
+  }
+  return transformed_goal;
+}
+
 bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
 {
   // Keep full path if we are within tolerance of the inversion pose

nav2_mppi_controller/test/critic_manager_test.cpp

@@ -103,10 +103,11 @@ TEST(CriticManagerTests, BasicCriticOperations)
   models::ControlSequence control_sequence;
   models::Trajectories generated_trajectories;
   models::Path path;
+  geometry_msgs::msg::Pose goal;
   xt::xtensor<float, 1> costs;
   float model_dt = 0.1;
   CriticData data =
-  {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr,
+  {state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
     std::nullopt, std::nullopt};
 
   data.fail_flag = true;