new param to disable collision checking in DriveOnHeading and BackUp actions (ros-navigation#4785)

david-wb · David Brown · masf7g · commit 43c810bd9251 · 2024-12-19T11:11:44.000-05:00
* add parameter to the DriveOnHeading and BackUp actions to disable collision checking

Signed-off-by: David Brown &lt;david.brown@inria.fr&gt;

* update commander api

Signed-off-by: David Brown &lt;david.brown@inria.fr&gt;

* use a less verbose param name

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

* fix flake8 error

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

* rename param

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

* set defaults in action messages and popular instance field for param

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

* populate field in backup action as well

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

* also add the new param to the spin action

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

* fix incorrect type

Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;

---------

Signed-off-by: David Brown &lt;david.brown@inria.fr&gt;
Signed-off-by: David Brown &lt;davewbrwn@gmail.com&gt;
Co-authored-by: David Brown &lt;david.brown@inria.fr&gt;
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp
@@ -80,6 +80,7 @@ class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
         BT::InputPort<double>("backup_dist", 0.15, "Distance to backup"),
         BT::InputPort<double>("backup_speed", 0.025, "Speed at which to backup"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for reversing"),
+        BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The back up behavior server error code")
       });
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp
@@ -59,6 +59,7 @@ class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeadi
         BT::InputPort<double>("dist_to_travel", 0.15, "Distance to travel"),
         BT::InputPort<double>("speed", 0.025, "Speed at which to travel"),
         BT::InputPort<double>("time_allowance", 10.0, "Allowed time for driving on heading"),
+        BT::InputPort<bool>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<Action::Result::_error_code_type>(
           "error_code_id", "The drive on heading behavior server error code")
       });
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
@@ -64,6 +64,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>("disable_collision_checks", false, "Disable collision checking"),
         BT::OutputPort<ActionResult::_error_code_type>(
           "error_code_id", "The spin behavior error code")
       });
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -13,6 +13,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="disable_collision_checks">Disable collision checking</input_port>
       <output_port name="error_code_id">"Back up error code"</output_port>
     </Action>
 
@@ -22,6 +23,7 @@
       <input_port name="time_allowance">Allowed time for reversing</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="disable_collision_checks">Disable collision checking</input_port>
       <output_port name="error_code_id">"Drive on heading error code"</output_port>
     </Action>
 
@@ -193,6 +195,7 @@
       <input_port name="time_allowance">Allowed time for spinning</input_port>
       <input_port name="server_name">Server name</input_port>
       <input_port name="server_timeout">Server timeout</input_port>
+      <input_port name="disable_collision_checks">Disable collision checking</input_port>
       <output_port name="error_code_id">Spin error code</output_port>
     </Action>
 
diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp
@@ -36,13 +36,16 @@ void nav2_behavior_tree::BackUpAction::initialize()
   getInput("backup_speed", speed);
   double time_allowance;
   getInput("time_allowance", time_allowance);
+  bool disable_collision_checks;
+  getInput("disable_collision_checks", disable_collision_checks);
 
   // Populate the input message
   goal_.target.x = dist;
   goal_.target.y = 0.0;
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  goal_.disable_collision_checks = disable_collision_checks;
 }
 
 void BackUpAction::on_tick()
diff --git a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp
@@ -37,13 +37,16 @@ void DriveOnHeadingAction::initialize()
   getInput("speed", speed);
   double time_allowance;
   getInput("time_allowance", time_allowance);
+  bool disable_collision_checks;
+  getInput("disable_collision_checks", disable_collision_checks);
 
   // Populate the input message
   goal_.target.x = dist;
   goal_.target.y = 0.0;
   goal_.target.z = 0.0;
   goal_.speed = speed;
   goal_.time_allowance = rclcpp::Duration::from_seconds(time_allowance);
+  goal_.disable_collision_checks = disable_collision_checks;
   initalized_ = true;
 }
 
diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp
@@ -131,18 +131,20 @@ TEST_F(BackUpActionTestFixture, test_ports)
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 0.15);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.025);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);
 
   xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <BackUp backup_dist="2" backup_speed="0.26" />
+            <BackUp backup_dist="2" backup_speed="0.26" disable_collision_checks="true" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_dist"), 2.0);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("backup_speed"), 0.26);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
 }
 
 TEST_F(BackUpActionTestFixture, test_tick)
diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp
@@ -128,18 +128,20 @@ TEST_F(DriveOnHeadingActionTestFixture, test_ports)
   EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 0.15);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.025);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("time_allowance"), 10.0);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);
 
   xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <DriveOnHeading dist_to_travel="2" speed="0.26" />
+            <DriveOnHeading dist_to_travel="2" speed="0.26" disable_collision_checks="true" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("dist_to_travel"), 2.0);
   EXPECT_EQ(tree_->rootNode()->getInput<double>("speed"), 0.26);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
 }
 
 TEST_F(DriveOnHeadingActionTestFixture, test_tick)
diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp
@@ -130,17 +130,19 @@ TEST_F(SpinActionTestFixture, test_ports)
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("spin_dist"), 1.57);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), false);
 
   xml_txt =
     R"(
       <root BTCPP_format="4">
         <BehaviorTree ID="MainTree">
-            <Spin spin_dist="3.14" />
+            <Spin spin_dist="3.14" disable_collision_checks="true" />
         </BehaviorTree>
       </root>)";
 
   tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
   EXPECT_EQ(tree_->rootNode()->getInput<double>("spin_dist"), 3.14);
+  EXPECT_EQ(tree_->rootNode()->getInput<bool>("disable_collision_checks"), true);
 }
 
 TEST_F(SpinActionTestFixture, test_tick)
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
@@ -47,6 +47,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     feedback_(std::make_shared<typename ActionT::Feedback>()),
     command_x_(0.0),
     command_speed_(0.0),
+    command_disable_collision_checks_(false),
     simulate_ahead_time_(0.0)
   {
   }
@@ -76,6 +77,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     command_x_ = command->target.x;
     command_speed_ = command->speed;
     command_time_allowance_ = command->time_allowance;
+    command_disable_collision_checks_ = command->disable_collision_checks;
 
     end_time_ = this->clock_->now() + command_time_allowance_;
 
@@ -168,6 +170,10 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     const geometry_msgs::msg::Twist & cmd_vel,
     geometry_msgs::msg::Pose2D & pose2d)
   {
+    if (command_disable_collision_checks_) {
+      return true;
+    }
+
     // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
     int cycle_count = 0;
     double sim_position_change;
@@ -229,6 +235,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
   geometry_msgs::msg::PoseStamped initial_pose_;
   double command_x_;
   double command_speed_;
+  bool command_disable_collision_checks_;
   rclcpp::Duration command_time_allowance_{0, 0};
   rclcpp::Time end_time_;
   double simulate_ahead_time_;
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp
@@ -89,6 +89,7 @@ class Spin : public TimedBehavior<SpinAction>
   double max_rotational_vel_;
   double rotational_acc_lim_;
   double cmd_yaw_;
+  bool cmd_disable_collision_checks_;
   double prev_yaw_;
   double relative_yaw_;
   double simulate_ahead_time_;
diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp
@@ -30,6 +30,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> comma
   command_x_ = -std::fabs(command->target.x);
   command_speed_ = -std::fabs(command->speed);
   command_time_allowance_ = command->time_allowance;
+  command_disable_collision_checks_ = command->disable_collision_checks;
 
   end_time_ = this->clock_->now() + command_time_allowance_;
 
diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp
@@ -91,6 +91,7 @@ ResultStatus Spin::onRun(const std::shared_ptr<const SpinActionGoal> command)
     cmd_yaw_);
 
   command_time_allowance_ = command->time_allowance;
+  cmd_disable_collision_checks_ = command->disable_collision_checks;
   end_time_ = this->clock_->now() + command_time_allowance_;
 
   return ResultStatus{Status::SUCCEEDED, SpinActionResult::NONE};
@@ -164,6 +165,10 @@ bool Spin::isCollisionFree(
   const geometry_msgs::msg::Twist & cmd_vel,
   geometry_msgs::msg::Pose2D & pose2d)
 {
+  if (cmd_disable_collision_checks_) {
+    return true;
+  }
+
   // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
   int cycle_count = 0;
   double sim_position_change;
diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action
@@ -3,6 +3,7 @@
 geometry_msgs/Point target
 float32 speed
 builtin_interfaces/Duration time_allowance
+bool disable_collision_checks False
 ---
 #result definition
 
diff --git a/nav2_msgs/action/DriveOnHeading.action b/nav2_msgs/action/DriveOnHeading.action
@@ -3,6 +3,7 @@
 geometry_msgs/Point target
 float32 speed
 builtin_interfaces/Duration time_allowance
+bool disable_collision_checks False
 ---
 #result definition
 
diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action
@@ -1,6 +1,7 @@
 #goal definition
 float32 target_yaw
 builtin_interfaces/Duration time_allowance
+bool disable_collision_checks False
 ---
 #result definition
 
diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md
@@ -23,8 +23,8 @@ New as of September 2023: the simple navigator constructor will accept a `namesp
 | goToPose(pose, behavior_tree='')  | Requests the robot to drive to a pose (`PoseStamped`).                     |
 | followWaypoints(poses)            | Requests the robot to follow a set of waypoints (list of `PoseStamped`). This will execute the specific `TaskExecutor` at each pose.   |
 | followPath(path, controller_id='', goal_checker_id='') | Requests the robot to follow a path from a starting to a goal `PoseStamped`, `nav_msgs/Path`.     |
-| spin(spin_dist=1.57, time_allowance=10)   | Requests the robot to performs an in-place rotation by a given angle.      |
-| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10) | Requests the robot to back up by a given distance.         |
+| spin(spin_dist=1.57, time_allowance=10, disable_collision_checks=False)   | Requests the robot to performs an in-place rotation by a given angle.      |
+| backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10, disable_collision_checks=False) | Requests the robot to back up by a given distance.         |
 | cancelTask()                       | Cancel an ongoing task request.|
 | isTaskComplete()                   | Checks if task is complete yet, times out at `100ms`.  Returns `True` if completed and `False` if still going.                  |
 | getFeedback()                     | Gets feedback from task, returns action server feedback object. |
diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py
@@ -231,13 +231,14 @@ def followGpsWaypoints(self, gps_poses):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def spin(self, spin_dist=1.57, time_allowance=10):
+    def spin(self, spin_dist=1.57, time_allowance=10, disable_collision_checks=False):
         self.debug("Waiting for 'Spin' action server")
         while not self.spin_client.wait_for_server(timeout_sec=1.0):
             self.info("'Spin' action server not available, waiting...")
         goal_msg = Spin.Goal()
         goal_msg.target_yaw = spin_dist
         goal_msg.time_allowance = Duration(sec=time_allowance)
+        goal_msg.disable_collision_checks = disable_collision_checks
 
         self.info(f'Spinning to angle {goal_msg.target_yaw}....')
         send_goal_future = self.spin_client.send_goal_async(
@@ -253,14 +254,16 @@ def spin(self, spin_dist=1.57, time_allowance=10):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
+    def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10,
+               disable_collision_checks=False):
         self.debug("Waiting for 'Backup' action server")
         while not self.backup_client.wait_for_server(timeout_sec=1.0):
             self.info("'Backup' action server not available, waiting...")
         goal_msg = BackUp.Goal()
         goal_msg.target = Point(x=float(backup_dist))
         goal_msg.speed = backup_speed
         goal_msg.time_allowance = Duration(sec=time_allowance)
+        goal_msg.disable_collision_checks = disable_collision_checks
 
         self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
         send_goal_future = self.backup_client.send_goal_async(
@@ -276,14 +279,16 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
         self.result_future = self.goal_handle.get_result_async()
         return True
 
-    def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10):
+    def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10,
+                       disable_collision_checks=False):
         self.debug("Waiting for 'DriveOnHeading' action server")
         while not self.backup_client.wait_for_server(timeout_sec=1.0):
             self.info("'DriveOnHeading' action server not available, waiting...")
         goal_msg = DriveOnHeading.Goal()
         goal_msg.target = Point(x=float(dist))
         goal_msg.speed = speed
         goal_msg.time_allowance = Duration(sec=time_allowance)
+        goal_msg.disable_collision_checks = disable_collision_checks
 
         self.info(f'Drive {goal_msg.target.x} m on heading at {goal_msg.speed} m/s....')
         send_goal_future = self.drive_on_heading_client.send_goal_async(
