spin without costmap

logivations · Apr 19, 2024 · 1da25a3 · 1da25a3
1 parent eac506a
commit 1da25a3
Show file tree

Hide file tree

Showing 5 changed files with 8 additions and 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
@@ -65,6 +65,7 @@ class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
         BT::InputPort<double>("spin_dist", 1.57, "Spin distance"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for spinning"),
         BT::InputPort<bool>("is_recovery", true, "True if recovery"),
+        BT::InputPort<bool>("check_local_costmap", true, "Check local costmap for collisions"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "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
@@ -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
@@ -90,6 +90,7 @@ class Spin : public TimedBehavior<SpinAction>
   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
@@ -92,6 +92,7 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> 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
@@ -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