Revert "Add goal pose to CriticData (#4812)" (#5527)

This reverts commit ad2f960.

Author: SteveMacenski

nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp

@@ -32,15 +32,14 @@ namespace mppi
 
 /**
  * @struct mppi::CriticData
- * @brief Data to pass to critics for scoring, including state, trajectories,
- * pruned path, global goal, costs, and important parameters to share
+ * @brief Data to pass to critics for scoring, including state, trajectories, path, 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,
-    const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
+    nav2_core::GoalChecker * goal_checker);
 
   /**
    * @brief Get the trajectories generated in a cycle for visualization
@@ -132,8 +132,7 @@ class Optimizer
   void prepare(
     const geometry_msgs::msg::PoseStamped & robot_pose,
     const geometry_msgs::msg::Twist & robot_speed,
-    const nav_msgs::msg::Path & plan,
-    const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
+    const nav_msgs::msg::Path & plan, nav2_core::GoalChecker * goal_checker);
 
   /**
    * @brief Obtain the main controller's parameters
@@ -251,12 +250,10 @@ 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_, goal_,
-    costs_, settings_.model_dt, false, nullptr, nullptr,
+  CriticData critics_data_ =
+  {state_, generated_trajectories_, path_, 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,13 +89,6 @@ 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
-   * @param stamp Time to get the goal pose at
-   * @return Transformed goal pose
-   */
-  geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
-
 protected:
   /**
     * @brief Transform a pose to another frame

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -195,23 +195,27 @@ 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 goal Goal pose
+ * @param path Path to retreive goal pose from
  * @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 geometry_msgs::msg::Pose & goal)
+  const models::Path & path)
 {
+  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.position.x;
-    auto dy = robot.position.y - goal.position.y;
+    auto dx = robot.position.x - goal_x;
+    auto dy = robot.position.y - goal_y;
 
     auto dist_sq = dx * dx + dy * dy;
 
@@ -227,19 +231,24 @@ 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 goal Goal pose
+ * @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 geometry_msgs::msg::Pose & robot,
-  const geometry_msgs::msg::Pose & goal)
+  const models::Path & path)
 {
-  const double & dist_sq =
-    std::pow(goal.position.x - robot.position.x, 2) +
-    std::pow(goal.position.y - robot.position.y, 2);
+  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 float pose_tolerance_sq = pose_tolerance * pose_tolerance;
+  auto dist_sq = dx * dx + dy * dy;
 
   if (dist_sq < pose_tolerance_sq) {
     return true;

nav2_mppi_controller/src/controller.cpp

@@ -92,15 +92,13 @@ 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(robot_pose.header.stamp).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, goal_checker);
+    optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker);
 
 #ifdef BENCHMARK_TESTING
   auto end = std::chrono::system_clock::now();

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -132,7 +132,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.goal)) {
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
     near_goal = true;
   }
 

nav2_mppi_controller/src/critics/goal_angle_critic.cpp

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

nav2_mppi_controller/src/critics/goal_critic.cpp

@@ -36,13 +36,15 @@ void GoalCritic::initialize()
 void GoalCritic::score(CriticData & data)
 {
   if (!enabled_ || !utils::withinPositionGoalTolerance(
-      threshold_to_consider_, data.state.pose.pose, data.goal))
+      threshold_to_consider_, data.state.pose.pose, data.path))
   {
     return;
   }
 
-  const auto & goal_x = data.goal.position.x;
-  const auto & goal_y = data.goal.position.y;
+  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 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

@@ -138,7 +138,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.goal)) {
+  if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {
     near_goal = true;
   }
 

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -46,8 +46,8 @@ 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.goal))
+  if (!enabled_ ||
+    utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path))
   {
     return;
   }