From 4e87ee0eca9d7eec691899f4b3507d75aeb34994 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Mon, 18 May 2020 22:13:15 +0100 Subject: [PATCH] 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 --- .../condition/goal_reached_condition.cpp | 5 ++++- .../plugins/decorator/distance_controller.cpp | 17 ++++++++++++++--- .../plugins/decorator/distance_controller.hpp | 1 + nav2_bt_navigator/src/bt_navigator.cpp | 1 + .../costmap_topic_collision_checker.hpp | 4 +++- .../src/costmap_topic_collision_checker.cpp | 9 +++++++-- .../include/nav2_recoveries/recovery.hpp | 2 ++ .../include/nav2_recoveries/recovery_server.hpp | 2 ++ nav2_recoveries/plugins/back_up.cpp | 4 ++-- nav2_recoveries/plugins/spin.cpp | 4 ++-- nav2_recoveries/src/recovery_server.cpp | 7 ++++++- 11 files changed, 44 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index 2e39eca9f2..e02598195e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -62,6 +62,8 @@ class GoalReachedCondition : public BT::ConditionNode node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); tf_ = config().blackboard->get>("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 diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index e259026462..ce3a61b450 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -44,6 +44,8 @@ DistanceController::DistanceController( getInput("robot_base_frame", robot_base_frame_); node_ = config().blackboard->get("node"); tf_ = config().blackboard->get>("tf_buffer"); + + node_->get_parameter("transform_tolerance", transform_tolerance_); } inline BT::NodeStatus DistanceController::tick() @@ -51,7 +53,10 @@ 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; } diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/plugins/decorator/distance_controller.hpp index 9e83994d6b..299833f555 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -50,6 +50,7 @@ class DistanceController : public BT::DecoratorNode rclcpp::Node::SharedPtr node_; std::shared_ptr tf_; + double transform_tolerance_; geometry_msgs::msg::PoseStamped start_pose_; double distance_; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index db0843abad..8b62c9b9f3 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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")); } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 4a511092a6..e4e374ce02 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp +++ b/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_; }; diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp index 610528cb4b..e81f6ab764 100644 --- a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp +++ b/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 & 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."); } diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 002ebb28bf..a486f190a0 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/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( node_, recovery_name_, @@ -140,6 +141,7 @@ class Recovery : public nav2_core::Recovery double cycle_frequency_; double enabled_; + double transform_tolerance_; void execute() { diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 0eff75fb77..f86cc38d8f 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -60,6 +60,8 @@ class RecoveryServer : public nav2_util::LifecycleNode std::unique_ptr costmap_sub_; std::unique_ptr footprint_sub_; std::shared_ptr collision_checker_; + + double transform_tolerance_; }; } // namespace recovery_server diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index d8d75052ae..2faaf03000 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -54,7 +54,7 @@ Status BackUp::onRun(const std::shared_ptr 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 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; } diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 119fa09a56..f96aaaaa74 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -71,7 +71,7 @@ void Spin::onConfigure() Status Spin::onRun(const std::shared_ptr 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 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; } diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index 58b2b240de..3a40c4c732 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/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( shared_from_this(), costmap_topic); footprint_sub_ = std::make_unique( shared_from_this(), footprint_topic, 1.0); collision_checker_ = std::make_shared( - *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_);