Add goal pose to CriticData (#4812)

* Add goal pose to CriticData

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Pass goal pose directly to withinPositionGoalTolerance

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix condition not

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Add goal positions to tests

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Use plan stamp

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Use float instead of auto

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Set pose frame id in test

Signed-off-by: redvinaa <redvinaa@gmail.com>

* Fix frame id in test vol 2

Signed-off-by: redvinaa <redvinaa@gmail.com>

---------

Signed-off-by: redvinaa <redvinaa@gmail.com>

Author: redvinaa

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

@@ -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/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.state.pose.pose, data.goal)) {
     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.path))
+      threshold_to_consider_, data.state.pose.pose, data.goal))
   {
     return;
   }

nav2_mppi_controller/src/critics/goal_critic.cpp

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