Fix some parameters not being passed to getCurrentPose

Signed-off-by: ymd-stella <[email protected]>

Author: ymd-stella

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -82,7 +82,7 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     geometry_msgs::msg::PoseStamped current_pose;
 
-    if (!nav2_util::getCurrentPose(current_pose, *tf_, "map", "base_link", transform_tolerance_)) {
+    if (!nav2_util::getCurrentPose(current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return false;
     }

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -140,6 +140,7 @@ class BtNavigator : public nav2_util::LifecycleNode
   rclcpp::Time start_time_;
   std::string robot_frame_;
   std::string global_frame_;
+  double transform_tolerance_;
 };
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/src/bt_navigator.cpp

@@ -110,6 +110,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array();
   global_frame_ = get_parameter("global_frame").as_string();
   robot_frame_ = get_parameter("robot_base_frame").as_string();
+  transform_tolerance_ = get_parameter("transform_tolerance").as_double();
 
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
@@ -242,7 +243,7 @@ BtNavigator::navigateToPose()
 
       // action server feedback (pose, duration of task,
       // number of recoveries, and distance remaining to goal)
-      nav2_util::getCurrentPose(feedback_msg->current_pose, *tf_);
+      nav2_util::getCurrentPose(feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_);
 
       geometry_msgs::msg::PoseStamped goal_pose;
       blackboard_->get("goal", goal_pose);