Parameterization of the tf_tol argument for getCurrentPose calls

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -62,6 +62,8 @@ class GoalReachedCondition : public BT::ConditionNode
     node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
     tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
+    node_->get_parameter("transform_tolerance", transform_tolerance_);
+
     initialized_ = true;
   }
 
@@ -70,7 +72,7 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     geometry_msgs::msg::PoseStamped current_pose;
 
-    if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
+    if (!nav2_util::getCurrentPose(current_pose, *tf_, "map", "base_link", transform_tolerance_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return false;
     }
@@ -105,6 +107,7 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  double transform_tolerance_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_bt_navigator/src/bt_navigator.cpp

@@ -54,6 +54,7 @@ BtNavigator::BtNavigator()
   // Declare this node's parameters
   declare_parameter("bt_xml_filename");
   declare_parameter("plugin_lib_names", plugin_libs);
+  declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1));
 }
 
 BtNavigator::~BtNavigator()

nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp

@@ -44,7 +44,8 @@ class CollisionChecker
     FootprintSubscriber & footprint_sub,
     tf2_ros::Buffer & tf,
     std::string name = "collision_checker",
-    std::string global_frame = "map");
+    std::string global_frame = "map",
+    double transform_tolerance = 0.1);
 
   ~CollisionChecker();
 
@@ -68,6 +69,7 @@ class CollisionChecker
   tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;
+  double transform_tolerance_;
 };
 }  // namespace nav2_costmap_2d
 

nav2_costmap_2d/src/collision_checker.cpp

@@ -34,12 +34,14 @@ CollisionChecker::CollisionChecker(
   FootprintSubscriber & footprint_sub,
   tf2_ros::Buffer & tf,
   std::string name,
-  std::string global_frame)
+  std::string global_frame,
+  double transform_tolerance)
 : name_(name),
   global_frame_(global_frame),
   tf_(tf),
   costmap_sub_(costmap_sub),
-  footprint_sub_(footprint_sub)
+  footprint_sub_(footprint_sub),
+  transform_tolerance_(transform_tolerance)
 {
 }
 
@@ -173,7 +175,10 @@ void CollisionChecker::unorientFootprint(
   std::vector<geometry_msgs::msg::Point> & reset_footprint)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, tf_, global_frame_)) {
+  if (!nav2_util::getCurrentPose(
+      current_pose, tf_, global_frame_, "base_link",
+      transform_tolerance_))
+  {
     throw CollisionCheckerException("Robot pose unavailable.");
   }
 

nav2_recoveries/include/nav2_recoveries/recovery.hpp

@@ -97,6 +97,7 @@ class Recovery : public nav2_core::Recovery
     tf_ = tf;
 
     node_->get_parameter("cycle_frequency", cycle_frequency_);
+    node_->get_parameter("transform_tolerance", transform_tolerance_);
 
     action_server_ = std::make_shared<ActionServer>(
       node_, recovery_name_,
@@ -140,6 +141,7 @@ class Recovery : public nav2_core::Recovery
 
   double cycle_frequency_;
   double enabled_;
+  double transform_tolerance_;
 
   void execute()
   {

nav2_recoveries/include/nav2_recoveries/recovery_server.hpp

@@ -60,6 +60,8 @@ class RecoveryServer : public nav2_util::LifecycleNode
   std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
   std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
   std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+
+  double transform_tolerance_;
 };
 
 }  // namespace recovery_server

nav2_recoveries/plugins/back_up.cpp

@@ -53,7 +53,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
   command_x_ = command->target.x;
   command_speed_ = command->speed;
 
-  if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom", "base_link", transform_tolerance_)) {
     RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available.");
     return Status::FAILED;
   }
@@ -64,7 +64,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
 Status BackUp::onCycleUpdate()
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom", "base_link", transform_tolerance_)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }

nav2_recoveries/plugins/spin.cpp

@@ -70,7 +70,7 @@ void Spin::onConfigure()
 Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom", "base_link", transform_tolerance_)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
@@ -88,7 +88,7 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 Status Spin::onCycleUpdate()
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom", "base_link", transform_tolerance_)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }

nav2_recoveries/src/recovery_server.cpp

@@ -45,6 +45,10 @@ RecoveryServer::RecoveryServer()
   declare_parameter(
     "plugin_types",
     rclcpp::ParameterValue(plugin_types));
+
+  declare_parameter(
+    "transform_tolerance",
+    rclcpp::ParameterValue(0.1));
 }
 
 
@@ -67,12 +71,13 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   std::string costmap_topic, footprint_topic;
   this->get_parameter("costmap_topic", costmap_topic);
   this->get_parameter("footprint_topic", footprint_topic);
+  this->get_parameter("transform_tolerance", transform_tolerance_);
   costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
     shared_from_this(), costmap_topic);
   footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
     shared_from_this(), footprint_topic, 1.0);
   collision_checker_ = std::make_shared<nav2_costmap_2d::CollisionChecker>(
-    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom");
+    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom", transform_tolerance_);
 
   this->get_parameter("plugin_names", plugin_names_);
   this->get_parameter("plugin_types", plugin_types_);