From 1da25a349f88003491e34a5e427d43707b0c9f0c Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Fri, 19 Apr 2024 21:07:16 +0200 Subject: [PATCH] spin without costmap --- .../include/nav2_behavior_tree/plugins/action/spin_action.hpp | 1 + nav2_behavior_tree/plugins/action/spin_action.cpp | 3 +++ nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp | 1 + nav2_behaviors/plugins/spin.cpp | 3 ++- nav2_msgs/action/Spin.action | 1 + 5 files changed, 8 insertions(+), 1 deletion(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp index d9520a6e5b..509193b4a3 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/spin_action.hpp @@ -65,6 +65,7 @@ class SpinAction : public BtActionNode BT::InputPort("spin_dist", 1.57, "Spin distance"), BT::InputPort("time_allowance", 10.0, "Allowed time for spinning"), BT::InputPort("is_recovery", true, "True if recovery"), + BT::InputPort("check_local_costmap", true, "Check local costmap for collisions"), BT::OutputPort( "error_code_id", "The spin behavior error code") }); diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index d3e8457ec1..df32d9195c 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -37,6 +37,9 @@ void SpinAction::initialize() goal_.target_yaw = dist; goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance); getInput("is_recovery", is_recovery_); + bool check_local_costmap; + getInput("check_local_costmap", check_local_costmap); + goal_.check_local_costmap = check_local_costmap; initialized_ = true; } diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp index e9388e359c..5ec1558bbf 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp @@ -90,6 +90,7 @@ class Spin : public TimedBehavior double prev_yaw_; double relative_yaw_; double simulate_ahead_time_; + bool check_local_costmap_; rclcpp::Duration command_time_allowance_{0, 0}; rclcpp::Time end_time_; }; diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 44093962b6..bb94fee64a 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -92,6 +92,7 @@ ResultStatus Spin::onRun(const std::shared_ptr command) command_time_allowance_ = command->time_allowance; end_time_ = steady_clock_.now() + command_time_allowance_; + check_local_costmap_ = command->check_local_costmap; return ResultStatus{Status::SUCCEEDED, SpinActionGoal::NONE}; } @@ -146,7 +147,7 @@ ResultStatus Spin::onCycleUpdate() pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { + if (check_local_costmap_ && !isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin"); return ResultStatus{Status::FAILED, SpinActionGoal::COLLISION_AHEAD}; diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 036cd1b8f5..73d436cfe7 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -9,6 +9,7 @@ uint16 COLLISION_AHEAD=703 #goal definition float32 target_yaw builtin_interfaces/Duration time_allowance +bool check_local_costmap --- #result definition builtin_interfaces/Duration total_elapsed_time