Use transformPoseInTargetFrame in all controllers

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Author: mini-1235

nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp

@@ -98,17 +98,6 @@ class PathHandler
   geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
 
 protected:
-  /**
-    * @brief Transform a pose to another frame
-    * @param frame Frame to transform to
-    * @param in_pose Input pose
-    * @param out_pose Output pose
-    * @return Bool if successful
-    */
-  bool transformPose(
-    const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
-    geometry_msgs::msg::PoseStamped & out_pose) const;
-
   /**
     * @brief Get largest dimension of costmap (radially)
     * @return Max distance from center of costmap to edge

nav2_mppi_controller/src/path_handler.cpp

@@ -17,6 +17,7 @@
 #include "nav2_mppi_controller/tools/path_handler.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace mppi
 {
@@ -85,7 +86,8 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
     geometry_msgs::msg::PoseStamped costmap_plan_pose;
     global_plan_pose->header.stamp = global_pose.header.stamp;
     global_plan_pose->header.frame_id = global_plan_.header.frame_id;
-    transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
+    nav2_util::transformPoseInTargetFrame(*global_plan_pose, costmap_plan_pose, *tf_buffer_,
+        costmap_->getGlobalFrameID(), transform_tolerance_);
 
     // Check if pose is inside the costmap
     if (!costmap_->getCostmap()->worldToMap(
@@ -109,7 +111,9 @@ geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
   }
 
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) {
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_buffer_,
+      global_plan_up_to_inversion_.header.frame_id, transform_tolerance_))
+  {
     throw nav2_core::ControllerTFError(
             "Unable to transform robot pose into global plan's frame");
   }
@@ -142,27 +146,6 @@ nav_msgs::msg::Path PathHandler::transformPath(
   return transformed_plan;
 }
 
-bool PathHandler::transformPose(
-  const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
-  geometry_msgs::msg::PoseStamped & out_pose) const
-{
-  if (in_pose.header.frame_id == frame) {
-    out_pose = in_pose;
-    return true;
-  }
-
-  try {
-    tf_buffer_->transform(
-      in_pose, out_pose, frame,
-      tf2::durationFromSec(transform_tolerance_));
-    out_pose.header.frame_id = frame;
-    return true;
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what());
-  }
-  return false;
-}
-
 double PathHandler::getMaxCostmapDist()
 {
   const auto & costmap = costmap_->getCostmap();
@@ -196,7 +179,9 @@ geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
     throw nav2_core::ControllerTFError("Goal pose has an empty frame_id");
   }
   geometry_msgs::msg::PoseStamped transformed_goal;
-  if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) {
+  if (!nav2_util::transformPoseInTargetFrame(goal, transformed_goal, *tf_buffer_,
+      costmap_->getGlobalFrameID(), transform_tolerance_))
+  {
     throw nav2_core::ControllerTFError("Unable to transform goal pose into costmap frame");
   }
   return transformed_goal;

nav2_mppi_controller/test/path_handler_test.cpp

@@ -46,13 +46,6 @@ class PathHandlerWrapper : public PathHandler
     return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
   }
 
-  bool transformPoseWrapper(
-    const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
-    geometry_msgs::msg::PoseStamped & out_pose) const
-  {
-    return transformPose(frame, in_pose, out_pose);
-  }
-
   geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
     const geometry_msgs::msg::PoseStamped & pose)
   {
@@ -180,12 +173,10 @@ TEST(PathHandlerTests, TestTransforms)
     path.poses[i].header.frame_id = "map";
   }
 
-  geometry_msgs::msg::PoseStamped robot_pose, output_pose;
+  geometry_msgs::msg::PoseStamped robot_pose;
   robot_pose.header.frame_id = "odom";
   robot_pose.pose.position.x = 2.5;
 
-  EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose));
-  EXPECT_EQ(output_pose.pose.position.x, 2.5);
 
   EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error);
   handler.setPath(path);

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp

@@ -43,7 +43,7 @@ class PathHandler
    * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler
    */
   PathHandler(
-    tf2::Duration transform_tolerance,
+    double transform_tolerance,
     std::shared_ptr<tf2_ros::Buffer> tf,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros);
 
@@ -65,18 +65,6 @@ class PathHandler
     const geometry_msgs::msg::PoseStamped & pose,
     double max_robot_pose_search_dist, bool reject_unit_path = false);
 
-  /**
-   * @brief Transform a pose to another frame.
-   * @param frame Frame ID to transform to
-   * @param in_pose Pose input to transform
-   * @param out_pose transformed output
-   * @return bool if successful
-   */
-  bool transformPose(
-    const std::string frame,
-    const geometry_msgs::msg::PoseStamped & in_pose,
-    geometry_msgs::msg::PoseStamped & out_pose) const;
-
   void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;}
 
   nav_msgs::msg::Path getPlan() {return global_plan_;}
@@ -89,7 +77,7 @@ class PathHandler
   double getCostmapMaxExtent() const;
 
   rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")};
-  tf2::Duration transform_tolerance_;
+  double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
   nav_msgs::msg::Path global_plan_;

nav2_regulated_pure_pursuit_controller/src/path_handler.cpp

@@ -23,14 +23,15 @@
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav2_ros_common/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
+#include "nav2_util/robot_utils.hpp"
 
 namespace nav2_regulated_pure_pursuit_controller
 {
 
 using nav2_util::geometry_utils::euclidean_distance;
 
 PathHandler::PathHandler(
-  tf2::Duration transform_tolerance,
+  double transform_tolerance,
   std::shared_ptr<tf2_ros::Buffer> tf,
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
 : transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros)
@@ -60,7 +61,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
 
   // let's get the pose of the robot in the frame of the plan
   geometry_msgs::msg::PoseStamped robot_pose;
-  if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
+  if (!nav2_util::transformPoseInTargetFrame(pose, robot_pose, *tf_, global_plan_.header.frame_id,
+      transform_tolerance_))
+  {
     throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame");
   }
 
@@ -101,7 +104,9 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
       stamped_pose.header.frame_id = global_plan_.header.frame_id;
       stamped_pose.header.stamp = robot_pose.header.stamp;
       stamped_pose.pose = global_plan_pose.pose;
-      if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) {
+      if (!nav2_util::transformPoseInTargetFrame(stamped_pose, transformed_pose, *tf_,
+        costmap_ros_->getBaseFrameID(), transform_tolerance_))
+      {
         throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame");
       }
       transformed_pose.pose.position.z = 0.0;
@@ -128,24 +133,4 @@ nav_msgs::msg::Path PathHandler::transformGlobalPlan(
   return transformed_plan;
 }
 
-bool PathHandler::transformPose(
-  const std::string frame,
-  const geometry_msgs::msg::PoseStamped & in_pose,
-  geometry_msgs::msg::PoseStamped & out_pose) const
-{
-  if (in_pose.header.frame_id == frame) {
-    out_pose = in_pose;
-    return true;
-  }
-
-  try {
-    tf_->transform(in_pose, out_pose, frame, transform_tolerance_);
-    out_pose.header.frame_id = frame;
-    return true;
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what());
-  }
-  return false;
-}
-
 }  // namespace nav2_regulated_pure_pursuit_controller

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -61,7 +61,7 @@ void RegulatedPurePursuitController::configure(
 
   // Handles global path transformations
   path_handler_ = std::make_unique<PathHandler>(
-    tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_);
+    params_->transform_tolerance, tf_, costmap_ros_);
 
   // Checks for imminent collisions
   collision_checker_ = std::make_unique<CollisionChecker>(node, costmap_ros_, params_);