Fix #2186

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -140,6 +140,16 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   bool getRobotPose(geometry_msgs::msg::PoseStamped & global_pose);
 
+  /**
+   * @brief Transform the input_pose in the global frame of the costmap
+   * @param input_pose pose to be transformed
+   * @param transformed_pose pose transformed
+   * @return True if the pose was transformed successfully, false otherwise
+   */
+  bool transformPoseToGlobalFrame(
+    const geometry_msgs::msg::PoseStamped & input_pose,
+    geometry_msgs::msg::PoseStamped & transformed_pose);
+
   /** @brief Returns costmap name */
   std::string getName() const
   {

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -534,4 +534,19 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose)
     global_frame_, robot_base_frame_, transform_tolerance_);
 }
 
+bool
+Costmap2DROS::transformPoseToGlobalFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose)
+{
+  if (input_pose.header.frame_id == global_frame_) {
+    transformed_pose = input_pose;
+    return true;
+  } else {
+    return nav2_util::transformPoseInTargetFrame(
+      input_pose, transformed_pose, *tf_buffer_,
+      global_frame_, transform_tolerance_);
+  }
+}
+
 }  // namespace nav2_costmap_2d

nav2_planner/src/planner_server.cpp

@@ -241,26 +241,37 @@ PlannerServer::computePlan()
       return;
     }
 
+    // Changing the start and goal pose frame to the global_frame_ of costmap_ros_ if needed
+    geometry_msgs::msg::PoseStamped goal_pose = goal->goal;
+    if (!costmap_ros_->transformPoseToGlobalFrame(start, start) ||
+      !costmap_ros_->transformPoseToGlobalFrame(goal->goal, goal_pose))
+    {
+      RCLCPP_WARN(
+        get_logger(), "Could not transform the start or goal pose in the costmap frame");
+      action_server_->terminate_current();
+      return;
+    }
+
     if (action_server_->is_preempt_requested()) {
       goal = action_server_->accept_pending_goal();
     }
 
-    result->path = getPlan(start, goal->goal, goal->planner_id);
+    result->path = getPlan(start, goal_pose, goal->planner_id);
 
     if (result->path.poses.size() == 0) {
       RCLCPP_WARN(
         get_logger(), "Planning algorithm %s failed to generate a valid"
         " path to (%.2f, %.2f)", goal->planner_id.c_str(),
-        goal->goal.pose.position.x, goal->goal.pose.position.y);
+        goal_pose.pose.position.x, goal_pose.pose.position.y);
       action_server_->terminate_current();
       return;
     }
 
     RCLCPP_DEBUG(
       get_logger(),
       "Found valid path of size %lu to (%.2f, %.2f)",
-      result->path.poses.size(), goal->goal.pose.position.x,
-      goal->goal.pose.position.y);
+      result->path.poses.size(), goal_pose.pose.position.x,
+      goal_pose.pose.position.y);
 
     // Publish the plan for visualization purposes
     publishPlan(result->path);

nav2_util/include/nav2_util/robot_utils.hpp

@@ -36,6 +36,12 @@ bool getCurrentPose(
   tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
   const std::string robot_frame = "base_link", const double transform_timeout = 0.1);
 
+bool transformPoseInTargetFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
+  const double transform_timeout = 0.1);
+
 }  // end namespace nav2_util
 
 #endif  // NAV2_UTIL__ROBOT_UTILS_HPP_

nav2_util/src/robot_utils.cpp

@@ -28,39 +28,47 @@ bool getCurrentPose(
   tf2_ros::Buffer & tf_buffer, const std::string global_frame,
   const std::string robot_frame, const double transform_timeout)
 {
-  static rclcpp::Logger logger = rclcpp::get_logger("getCurrentPose");
-  geometry_msgs::msg::PoseStamped robot_pose;
-
   tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
-  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
-  robot_pose.header.frame_id = robot_frame;
-  robot_pose.header.stamp = rclcpp::Time();
+  global_pose.header.frame_id = robot_frame;
+  global_pose.header.stamp = rclcpp::Time();
+
+  return transformPoseInTargetFrame(
+    global_pose, global_pose, tf_buffer, global_frame, transform_timeout);
+}
+
+bool transformPoseInTargetFrame(
+  const geometry_msgs::msg::PoseStamped & input_pose,
+  geometry_msgs::msg::PoseStamped & transformed_pose,
+  tf2_ros::Buffer & tf_buffer, const std::string target_frame,
+  const double transform_timeout)
+{
+  static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
 
   try {
-    global_pose = tf_buffer.transform(
-      robot_pose, global_frame,
+    transformed_pose = tf_buffer.transform(
+      input_pose, target_frame,
       tf2::durationFromSec(transform_timeout));
     return true;
   } catch (tf2::LookupException & ex) {
     RCLCPP_ERROR(
       logger,
-      "No Transform available Error looking up robot pose: %s\n", ex.what());
+      "No Transform available Error looking up target frame: %s\n", ex.what());
   } catch (tf2::ConnectivityException & ex) {
     RCLCPP_ERROR(
       logger,
-      "Connectivity Error looking up robot pose: %s\n", ex.what());
+      "Connectivity Error looking up target frame: %s\n", ex.what());
   } catch (tf2::ExtrapolationException & ex) {
     RCLCPP_ERROR(
       logger,
-      "Extrapolation Error looking up robot pose: %s\n", ex.what());
+      "Extrapolation Error looking up target frame: %s\n", ex.what());
   } catch (tf2::TimeoutException & ex) {
     RCLCPP_ERROR(
       logger,
       "Transform timeout with tolerance: %.4f", transform_timeout);
   } catch (tf2::TransformException & ex) {
     RCLCPP_ERROR(
       logger, "Failed to transform from %s to %s",
-      global_frame.c_str(), robot_frame.c_str());
+      input_pose.header.frame_id.c_str(), target_frame.c_str());
   }
 
   return false;

nav2_util/test/test_robot_utils.cpp

@@ -29,4 +29,6 @@ TEST(RobotUtils, LookupExceptionError)
   geometry_msgs::msg::PoseStamped global_pose;
   tf2_ros::Buffer tf(node->get_clock());
   ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1));
+  global_pose.header.frame_id = "base_link";
+  ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1));
 }