From aa980bfbb02881e8693c4f81b10ec6074180ea5f Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Thu, 14 May 2020 06:06:44 +0100 Subject: [PATCH 1/8] parametrized collision_checker getCurrentPose timeout --- .../include/nav2_costmap_2d/collision_checker.hpp | 4 +++- nav2_costmap_2d/src/collision_checker.cpp | 11 ++++++++--- .../include/nav2_recoveries/recovery_server.hpp | 2 ++ nav2_recoveries/src/recovery_server.cpp | 7 ++++++- 4 files changed, 19 insertions(+), 5 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp index 13139e6cd7d..a73cc57c1d4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/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 diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp index 4e8d92c1278..02dbd2ee524 100644 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ b/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 & 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_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 026c3ecab4f..0771b9ee926 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/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index ecd23d4481f..bee56227cc5 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( + "tranceform_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_); From 877db4df0c471f55368885233e3d0f40b9747b65 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Thu, 14 May 2020 21:01:53 +0100 Subject: [PATCH 2/8] Parameterized tf_tol for spin and backup recoveries --- nav2_recoveries/plugins/back_up.cpp | 5 +++-- nav2_recoveries/plugins/back_up.hpp | 1 + nav2_recoveries/plugins/spin.cpp | 6 ++++-- nav2_recoveries/plugins/spin.hpp | 1 + 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 9706e6e82bf..416a08854a6 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -40,6 +40,7 @@ void BackUp::onConfigure() node_, "simulate_ahead_time", rclcpp::ParameterValue(2.0)); node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); + node_->get_parameter("transform_tolerance", transform_tolerance_); } Status BackUp::onRun(const std::shared_ptr command) @@ -53,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; } @@ -64,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/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index 4f04bd0352c..5acc48fc6d8 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -51,6 +51,7 @@ class BackUp : public Recovery double command_x_; double command_speed_; double simulate_ahead_time_; + double transform_tolerance_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 9afd6a2ac42..1ea78ccac51 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -65,12 +65,14 @@ void Spin::onConfigure() node_, "rotational_acc_lim", rclcpp::ParameterValue(3.2)); node_->get_parameter("rotational_acc_lim", rotational_acc_lim_); + + node_->get_parameter("transform_tolerance", transform_tolerance_); } 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; } @@ -88,7 +90,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/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 87b670e1004..807d04890f0 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -50,6 +50,7 @@ class Spin : public Recovery double prev_yaw_; double relative_yaw_; double simulate_ahead_time_; + double transform_tolerance_; }; } // namespace nav2_recoveries From 40eb239d59ca9e7e2d9a8bd1b7f706a410fb8b98 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Thu, 14 May 2020 21:23:43 +0100 Subject: [PATCH 3/8] Parameterized tf_tol for goal_reached_condition --- .../plugins/condition/goal_reached_condition.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c2cd02b8c..f4da2aace7e 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -62,6 +62,9 @@ class GoalReachedCondition : public BT::ConditionNode node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); tf_ = config().blackboard->get>("tf_buffer"); + node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); + node_->get_parameter("transform_tolerance", transform_tolerance_); + initialized_ = true; } @@ -70,7 +73,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 +108,7 @@ class GoalReachedCondition : public BT::ConditionNode bool initialized_; double goal_reached_tol_; + double transform_tolerance_; }; } // namespace nav2_behavior_tree From 6191970249d450cbd8341863c1c170bf166779f6 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Fri, 15 May 2020 01:15:23 +0100 Subject: [PATCH 4/8] moved transform_tolerance_ to recovery.hpp --- nav2_core/include/nav2_core/recovery.hpp | 3 +++ nav2_recoveries/plugins/back_up.hpp | 1 - nav2_recoveries/plugins/spin.hpp | 1 - 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index 1041d74d8e9..5c304ee3ff4 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -74,6 +74,9 @@ class Recovery // TODO(stevemacenski) evaluate design for recoveries to not host // their own servers and utilize a recovery server exposed action. // virtual bool executeRecovery() = 0; + +protected: + double transform_tolerance_; }; } // namespace nav2_core diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index 5acc48fc6d8..4f04bd0352c 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -51,7 +51,6 @@ class BackUp : public Recovery double command_x_; double command_speed_; double simulate_ahead_time_; - double transform_tolerance_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 807d04890f0..87b670e1004 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -50,7 +50,6 @@ class Spin : public Recovery double prev_yaw_; double relative_yaw_; double simulate_ahead_time_; - double transform_tolerance_; }; } // namespace nav2_recoveries From 59295e8641e9208d9307c3450c82fd31ae2f6a33 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Fri, 15 May 2020 01:31:06 +0100 Subject: [PATCH 5/8] moved transform_tolerance parameter declaration to bt_navigator --- nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp | 1 - nav2_bt_navigator/src/bt_navigator.cpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index f4da2aace7e..ffbd78eed80 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -62,7 +62,6 @@ class GoalReachedCondition : public BT::ConditionNode node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); tf_ = config().blackboard->get>("tf_buffer"); - node_->declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); node_->get_parameter("transform_tolerance", transform_tolerance_); initialized_ = true; diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 49d667c4fa8..12fb5af161a 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/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() From 7c0d15310db4ee4d553263ae9bb00707a5991512 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Fri, 15 May 2020 02:01:24 +0100 Subject: [PATCH 6/8] Fixed typo --- nav2_recoveries/src/recovery_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index bee56227cc5..1e4e5769d0e 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -47,7 +47,7 @@ RecoveryServer::RecoveryServer() rclcpp::ParameterValue(plugin_types)); declare_parameter( - "tranceform_tolerance", + "transform_tolerance", rclcpp::ParameterValue(0.1)); } From 24cafab931d6bd947780823024c6f333d9e31602 Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Fri, 15 May 2020 02:31:56 +0100 Subject: [PATCH 7/8] Fixed transform_tolerance_ location and transform_tolerance param declaration location --- nav2_core/include/nav2_core/recovery.hpp | 3 --- nav2_recoveries/include/nav2_recoveries/recovery.hpp | 2 ++ nav2_recoveries/plugins/back_up.cpp | 1 - nav2_recoveries/plugins/spin.cpp | 2 -- 4 files changed, 2 insertions(+), 6 deletions(-) diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index 5c304ee3ff4..1041d74d8e9 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -74,9 +74,6 @@ class Recovery // TODO(stevemacenski) evaluate design for recoveries to not host // their own servers and utilize a recovery server exposed action. // virtual bool executeRecovery() = 0; - -protected: - double transform_tolerance_; }; } // namespace nav2_core diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 80606e90ff1..211fab59ed1 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/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 416a08854a6..f39d95c295e 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -40,7 +40,6 @@ void BackUp::onConfigure() node_, "simulate_ahead_time", rclcpp::ParameterValue(2.0)); node_->get_parameter("simulate_ahead_time", simulate_ahead_time_); - node_->get_parameter("transform_tolerance", transform_tolerance_); } Status BackUp::onRun(const std::shared_ptr command) diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 1ea78ccac51..d5c6da1ef2b 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -65,8 +65,6 @@ void Spin::onConfigure() node_, "rotational_acc_lim", rclcpp::ParameterValue(3.2)); node_->get_parameter("rotational_acc_lim", rotational_acc_lim_); - - node_->get_parameter("transform_tolerance", transform_tolerance_); } Status Spin::onRun(const std::shared_ptr command) From 235f572bd2596d62b5a6f1110250849c0f04f20f Mon Sep 17 00:00:00 2001 From: Marwan Taher Date: Sat, 16 May 2020 00:18:51 +0100 Subject: [PATCH 8/8] Parameterized tf_tol for distance_controller.cpp --- .../plugins/decorator/distance_controller.cpp | 17 ++++++++++++++--- .../plugins/decorator/distance_controller.hpp | 1 + 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index e2590264623..ce3a61b4507 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 9e83994d6b7..299833f5550 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_;