Parameterization of the tf_tol argument for getCurrentPose calls (#1735)

* parametrized collision_checker getCurrentPose timeout

* Parameterized tf_tol for spin and backup recoveries

* Parameterized tf_tol for goal_reached_condition

* moved transform_tolerance_ to recovery.hpp

* moved transform_tolerance parameter declaration to bt_navigator

* Fixed typo

* Fixed transform_tolerance_ location and transform_tolerance param declaration location

* Parameterized tf_tol for distance_controller.cpp

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;
     }
@@ -101,6 +103,7 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  double transform_tolerance_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/decorator/distance_controller.cpp

@@ -44,14 +44,19 @@ DistanceController::DistanceController(
   getInput("robot_base_frame", robot_base_frame_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
+
+  node_->get_parameter("transform_tolerance", transform_tolerance_);
 }
 
 inline BT::NodeStatus DistanceController::tick()
 {
   if (status() == BT::NodeStatus::IDLE) {
     // Reset the starting position since we're starting a new iteration of
     // the distance controller (moving from IDLE to RUNNING)
-    if (!nav2_util::getCurrentPose(start_pose_, *tf_, global_frame_, robot_base_frame_)) {
+    if (!nav2_util::getCurrentPose(
+        start_pose_, *tf_, global_frame_, robot_base_frame_,
+        transform_tolerance_))
+    {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return BT::NodeStatus::FAILURE;
     }
@@ -62,7 +67,10 @@ inline BT::NodeStatus DistanceController::tick()
 
   // Determine distance travelled since we've started this iteration
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, global_frame_, robot_base_frame_)) {
+  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 BT::NodeStatus::FAILURE;
   }
@@ -85,7 +93,10 @@ inline BT::NodeStatus DistanceController::tick()
         return BT::NodeStatus::RUNNING;
 
       case BT::NodeStatus::SUCCESS:
-        if (!nav2_util::getCurrentPose(start_pose_, *tf_, global_frame_, robot_base_frame_)) {
+        if (!nav2_util::getCurrentPose(
+            start_pose_, *tf_, global_frame_, robot_base_frame_,
+            transform_tolerance_))
+        {
           RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
           return BT::NodeStatus::FAILURE;
         }

nav2_behavior_tree/plugins/decorator/distance_controller.hpp

@@ -50,6 +50,7 @@ class DistanceController : public BT::DecoratorNode
   rclcpp::Node::SharedPtr node_;
 
   std::shared_ptr<tf2_ros::Buffer> tf_;
+  double transform_tolerance_;
 
   geometry_msgs::msg::PoseStamped start_pose_;
   double distance_;

nav2_bt_navigator/src/bt_navigator.cpp

@@ -58,6 +58,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));
   declare_parameter("global_frame", std::string("map"));
   declare_parameter("robot_base_frame", std::string("base_link"));
 }

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -46,7 +46,8 @@ class CostmapTopicCollisionChecker
     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);
 
   ~CostmapTopicCollisionChecker() = default;
 
@@ -64,6 +65,7 @@ class CostmapTopicCollisionChecker
   tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;
+  double transform_tolerance_;
   FootprintCollisionChecker collision_checker_;
 };
 

nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

@@ -37,12 +37,14 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   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),
+  transform_tolerance_(transform_tolerance),
   collision_checker_(nullptr)
 {
 }
@@ -104,7 +106,10 @@ void CostmapTopicCollisionChecker::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::CostmapTopicCollisionChecker> collision_checker_;
+
+  double transform_tolerance_;
 };
 
 }  // namespace recovery_server

nav2_recoveries/plugins/back_up.cpp

@@ -54,7 +54,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;
   }
@@ -65,7 +65,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

@@ -71,7 +71,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;
   }
@@ -89,7 +89,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::CostmapTopicCollisionChecker>(
-    *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_);