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 index 28722a1fcc0..37484cdbd40 100644 --- 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 BT::InputPort("backup_dist", 0.15, "Distance to backup"), BT::InputPort("backup_speed", 0.025, "Speed at which to backup"), BT::InputPort("time_allowance", 10.0, "Allowed time for reversing"), + BT::InputPort("disable_collision_checks", false, "Disable collision checking"), BT::OutputPort( "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 index dd1a6f29086..67730ac51cd 100644 --- 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("dist_to_travel", 0.15, "Distance to travel"), BT::InputPort("speed", 0.025, "Speed at which to travel"), BT::InputPort("time_allowance", 10.0, "Allowed time for driving on heading"), + BT::InputPort("disable_collision_checks", false, "Disable collision checking"), BT::OutputPort( "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 index cad5e8ce4a8..711283fb076 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 @@ -64,6 +64,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("disable_collision_checks", false, "Disable collision checking"), BT::OutputPort( "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 index 67059def0eb..5eae2516f74 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -13,6 +13,7 @@ Allowed time for reversing Server name Server timeout + Disable collision checking "Back up error code" @@ -22,6 +23,7 @@ Allowed time for reversing Server name Server timeout + Disable collision checking "Drive on heading error code" @@ -201,6 +203,7 @@ Allowed time for spinning Server name Server timeout + Disable collision checking Spin error code diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 3df77f98d7c..070cc4a88e5 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -36,6 +36,8 @@ 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; @@ -43,6 +45,7 @@ void nav2_behavior_tree::BackUpAction::initialize() 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 index 03c00344141..c5fa780a4c9 100644 --- a/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp +++ b/nav2_behavior_tree/plugins/action/drive_on_heading_action.cpp @@ -37,6 +37,8 @@ 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; @@ -44,6 +46,7 @@ void DriveOnHeadingAction::initialize() 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/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 074eed93309..f18ab1b54b8 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -55,6 +55,14 @@ void RemoveInCollisionGoals::on_tick() BT::NodeStatus RemoveInCollisionGoals::on_completion( std::shared_ptr response) { + if (!response->success) { + RCLCPP_ERROR( + node_->get_logger(), + "GetCosts service call failed"); + setOutput("output_goals", input_goals_); + return BT::NodeStatus::FAILURE; + } + nav2_msgs::msg::PoseStampedArray valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 1ab9d2ff34c..6eb3c6e99e5 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -37,83 +37,83 @@ BT::NodeStatus RecoveryNode::tick() throw BT::BehaviorTreeException("Recovery Node '" + name() + "' must only have 2 children."); } - if (retry_count_ > number_of_retries_) { - halt(); - return BT::NodeStatus::FAILURE; - } setStatus(BT::NodeStatus::RUNNING); - TreeNode * child_node = children_nodes_[current_child_idx_]; - const BT::NodeStatus child_status = child_node->executeTick(); - - if (current_child_idx_ == 0) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - // If first child is skipped, the entire branch is considered skipped - halt(); - return BT::NodeStatus::SKIPPED; - - case BT::NodeStatus::SUCCESS: - // reset node and return success when first child returns success - halt(); - return BT::NodeStatus::SUCCESS; - - case BT::NodeStatus::RUNNING: - return BT::NodeStatus::RUNNING; - - case BT::NodeStatus::FAILURE: - { - if (retry_count_ < number_of_retries_) { - // halt first child and tick second child in next iteration - ControlNode::haltChild(0); - current_child_idx_ = 1; - return BT::NodeStatus::RUNNING; - } else { - // reset node and return failure when max retries has been exceeded - halt(); + while (current_child_idx_ < children_count && retry_count_ <= number_of_retries_) { + TreeNode * child_node = children_nodes_[current_child_idx_]; + const BT::NodeStatus child_status = child_node->executeTick(); + + if (current_child_idx_ == 0) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + // If first child is skipped, the entire branch is considered skipped + halt(); + return BT::NodeStatus::SKIPPED; + + case BT::NodeStatus::SUCCESS: + // reset node and return success when first child returns success + halt(); + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::FAILURE: + { + if (retry_count_ < number_of_retries_) { + // halt first child and tick second child in next iteration + ControlNode::haltChild(0); + current_child_idx_++; + break; + } else { + // reset node and return failure when max retries has been exceeded + halt(); + return BT::NodeStatus::FAILURE; + } + } + + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + + } else if (current_child_idx_ == 1) { + switch (child_status) { + case BT::NodeStatus::SKIPPED: + { + // if we skip the recovery (maybe a precondition fails), then we + // should assume that no recovery is possible. For this reason, + // we should return FAILURE and reset the index. + // This does not count as a retry. + current_child_idx_ = 0; + ControlNode::haltChild(1); return BT::NodeStatus::FAILURE; } - } - - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - - } else if (current_child_idx_ == 1) { - switch (child_status) { - case BT::NodeStatus::SKIPPED: - { - // if we skip the recovery (maybe a precondition fails), then we - // should assume that no recovery is possible. For this reason, - // we should return FAILURE and reset the index. - // This does not count as a retry. - current_child_idx_ = 0; - ControlNode::haltChild(1); + case BT::NodeStatus::RUNNING: + return child_status; + + case BT::NodeStatus::SUCCESS: + { + // halt second child, increment recovery count, and tick first child in next iteration + ControlNode::haltChild(1); + retry_count_++; + current_child_idx_ = 0; + } + break; + + case BT::NodeStatus::FAILURE: + // reset node and return failure if second child fails + halt(); return BT::NodeStatus::FAILURE; - } - case BT::NodeStatus::RUNNING: - return child_status; - - case BT::NodeStatus::SUCCESS: - { - // halt second child, increment recovery count, and tick first child in next iteration - ControlNode::haltChild(1); - retry_count_++; - current_child_idx_ = 0; - return BT::NodeStatus::RUNNING; - } - case BT::NodeStatus::FAILURE: - // reset node and return failure if second child fails - halt(); - return BT::NodeStatus::FAILURE; + default: + throw BT::LogicError("A child node must never return IDLE"); + } // end switch + } + } // end while loop - default: - throw BT::LogicError("A child node must never return IDLE"); - } // end switch - } + // reset node and return failure halt(); - throw BT::LogicError("A recovery node has exactly 2 children and should never reach here"); + return BT::NodeStatus::FAILURE; } void RecoveryNode::halt() 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 index 6974b351d38..98484bde8cb 100644 --- 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(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 0.15); EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.025); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), false); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 2.0); EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.26); + EXPECT_EQ(tree_->rootNode()->getInput("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 index 99c3db72434..0c53fd54c8f 100644 --- 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("dist_to_travel"), 0.15); EXPECT_EQ(tree_->rootNode()->getInput("speed"), 0.025); EXPECT_EQ(tree_->rootNode()->getInput("time_allowance"), 10.0); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), false); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("dist_to_travel"), 2.0); EXPECT_EQ(tree_->rootNode()->getInput("speed"), 0.26); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), true); } TEST_F(DriveOnHeadingActionTestFixture, test_tick) diff --git a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp index b91300f7985..a1038ead686 100644 --- a/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_remove_in_collision_goals_action.cpp @@ -25,10 +25,10 @@ #include "utils/test_behavior_tree_fixture.hpp" -class RemoveInCollisionGoalsService : public TestService +class RemoveInCollisionGoalsSucessService : public TestService { public: - RemoveInCollisionGoalsService() + RemoveInCollisionGoalsSucessService() : TestService("/global_costmap/get_cost_global_costmap") {} @@ -40,9 +40,28 @@ class RemoveInCollisionGoalsService : public TestServicecosts = {100, 50, 5, 254}; + response->success = true; } }; +class RemoveInCollisionGoalsFailureService : public TestService +{ +public: + RemoveInCollisionGoalsFailureService() + : TestService("/local_costmap/get_cost_local_costmap") + {} + + virtual void handle_service( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + (void)request_header; + (void)request; + response->costs = {255, 50, 5, 254}; + response->success = false; + } +}; class RemoveInCollisionGoalsTestFixture : public ::testing::Test { @@ -86,7 +105,7 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test delete config_; config_ = nullptr; node_.reset(); - server_.reset(); + success_server_.reset(); factory_.reset(); } @@ -94,7 +113,8 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test { tree_.reset(); } - static std::shared_ptr server_; + static std::shared_ptr success_server_; + static std::shared_ptr failure_server_; protected: static rclcpp::Node::SharedPtr node_; @@ -106,12 +126,14 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test rclcpp::Node::SharedPtr RemoveInCollisionGoalsTestFixture::node_ = nullptr; BT::NodeConfiguration * RemoveInCollisionGoalsTestFixture::config_ = nullptr; -std::shared_ptr -RemoveInCollisionGoalsTestFixture::server_ = nullptr; +std::shared_ptr +RemoveInCollisionGoalsTestFixture::success_server_ = nullptr; +std::shared_ptr +RemoveInCollisionGoalsTestFixture::failure_server_ = nullptr; std::shared_ptr RemoveInCollisionGoalsTestFixture::factory_ = nullptr; std::shared_ptr RemoveInCollisionGoalsTestFixture::tree_ = nullptr; -TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) +TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_success) { // create tree std::string xml_txt = @@ -141,11 +163,13 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) config_->blackboard->set("goals", poses); - // tick until node succeeds - while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + // tick until node is not running + tree_->rootNode()->executeTick(); + while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) { tree_->rootNode()->executeTick(); } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); // check that it removed the point in range nav2_msgs::msg::PoseStampedArray output_poses; EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); @@ -156,6 +180,54 @@ TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals) EXPECT_EQ(output_poses.poses[2], poses.poses[2]); } +TEST_F(RemoveInCollisionGoalsTestFixture, test_tick_remove_in_collision_goals_fail) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new goal and set it on blackboard + std::vector poses; + poses.resize(4); + poses[0].pose.position.x = 0.0; + poses[0].pose.position.y = 0.0; + + poses[1].pose.position.x = 0.5; + poses[1].pose.position.y = 0.0; + + poses[2].pose.position.x = 1.0; + poses[2].pose.position.y = 0.0; + + poses[3].pose.position.x = 2.0; + poses[3].pose.position.y = 0.0; + + config_->blackboard->set("goals", poses); + + // tick until node is not running + tree_->rootNode()->executeTick(); + while (tree_->rootNode()->status() == BT::NodeStatus::RUNNING) { + tree_->rootNode()->executeTick(); + } + + // check that it failed and returned the original goals + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::FAILURE); + std::vector output_poses; + EXPECT_TRUE(config_->blackboard->get("goals", output_poses)); + + EXPECT_EQ(output_poses.size(), 4u); + EXPECT_EQ(output_poses[0], poses[0]); + EXPECT_EQ(output_poses[1], poses[1]); + EXPECT_EQ(output_poses[2], poses[2]); + EXPECT_EQ(output_poses[3], poses[3]); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); @@ -164,17 +236,24 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); // initialize service and spin on new thread - RemoveInCollisionGoalsTestFixture::server_ = - std::make_shared(); - std::thread server_thread([]() { - rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_); + RemoveInCollisionGoalsTestFixture::success_server_ = + std::make_shared(); + std::thread success_server_thread([]() { + rclcpp::spin(RemoveInCollisionGoalsTestFixture::success_server_); + }); + + RemoveInCollisionGoalsTestFixture::failure_server_ = + std::make_shared(); + std::thread failure_server_thread([]() { + rclcpp::spin(RemoveInCollisionGoalsTestFixture::failure_server_); }); int all_successful = RUN_ALL_TESTS(); // shutdown ROS rclcpp::shutdown(); - server_thread.join(); + success_server_thread.join(); + failure_server_thread.join(); return all_successful; } diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 2d88d6e2092..134e9dc9c12 100644 --- 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(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("spin_dist"), 1.57); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), false); xml_txt = R"( - + )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); EXPECT_EQ(tree_->rootNode()->getInput("spin_dist"), 3.14); + EXPECT_EQ(tree_->rootNode()->getInput("disable_collision_checks"), true); } TEST_F(SpinActionTestFixture, test_tick) diff --git a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp index 532fcad45b2..82927031429 100644 --- a/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp +++ b/nav2_behavior_tree/test/plugins/control/test_recovery_node.cpp @@ -105,7 +105,6 @@ TEST_F(RecoveryNodeTestFixture, test_failure_on_idle_child) first_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); first_child_->changeStatus(BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); second_child_->changeStatus(BT::NodeStatus::IDLE); EXPECT_THROW(bt_node_->executeTick(), BT::LogicError); } @@ -120,9 +119,8 @@ TEST_F(RecoveryNodeTestFixture, test_success_one_retry) first_child_->returnSuccessOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); } @@ -132,8 +130,8 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) // first child fails, second child fails first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::FAILURE); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); + EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); EXPECT_EQ(second_child_->status(), BT::NodeStatus::IDLE); @@ -141,8 +139,6 @@ TEST_F(RecoveryNodeTestFixture, test_failure_one_retry) first_child_->returnFailureOn(1); first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SUCCESS); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); @@ -162,7 +158,6 @@ TEST_F(RecoveryNodeTestFixture, test_skipping) // first child fails, second child skipped first_child_->changeStatus(BT::NodeStatus::FAILURE); second_child_->changeStatus(BT::NodeStatus::SKIPPED); - EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::RUNNING); EXPECT_EQ(bt_node_->executeTick(), BT::NodeStatus::FAILURE); EXPECT_EQ(bt_node_->status(), BT::NodeStatus::FAILURE); EXPECT_EQ(first_child_->status(), BT::NodeStatus::IDLE); diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index 0b74a044629..8a37f26a630 100644 --- 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 feedback_(std::make_shared()), 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 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 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; @@ -215,6 +221,7 @@ class DriveOnHeading : public TimedBehavior 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 index eeb84462220..7ef1e05489f 100644 --- 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 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 index 7a194874c7e..137ebb1c3b3 100644 --- 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 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 index 5965675f643..ce89eb34f23 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,6 +91,7 @@ ResultStatus Spin::onRun(const std::shared_ptr 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_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index a9f27a00bf0..7028cff6c37 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -825,15 +825,14 @@ void Costmap2DROS::getCostsCallback( unsigned int mx, my; Costmap2D * costmap = layered_costmap_->getCostmap(); - + response->success = true; for (const auto & pose : request->poses.poses) { geometry_msgs::msg::PoseStamped pose_transformed; - transformPoseToGlobalFrame(pose, pose_transformed); - bool in_bounds = costmap->worldToMap( - pose_transformed.pose.position.x, - pose_transformed.pose.position.y, mx, my); - - if (!in_bounds) { + if (!transformPoseToGlobalFrame(pose, pose_transformed)) { + RCLCPP_ERROR( + get_logger(), "Failed to transform, cannot get cost for pose (%.2f, %.2f)", + pose.pose.position.x, pose.pose.position.y); + response->success = false; response->costs.push_back(NO_INFORMATION); continue; } @@ -857,6 +856,15 @@ void Costmap2DROS::getCostsCallback( pose_transformed.pose.position.x, pose_transformed.pose.position.y); + bool in_bounds = costmap->worldToMap( + pose_transformed.pose.position.x, + pose_transformed.pose.position.y, mx, my); + + if (!in_bounds) { + response->success = false; + response->costs.push_back(LETHAL_OBSTACLE); + continue; + } // Get the cost at the map coordinates response->costs.push_back(static_cast(costmap->getCost(mx, my))); } diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index 9be1d56988f..a3bd0cf1afa 100644 --- 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 index a9bc79710d7..567fdc718ab 100644 --- 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 index 91288cbf205..5e0992ae9e0 100644 --- 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_msgs/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 43009661906..c782b01b987 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -3,4 +3,5 @@ bool use_footprint nav2_msgs/PoseStampedArray poses --- -float32[] costs \ No newline at end of file +float32[] costs +bool success \ No newline at end of file diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index 668fd9fe7c8..2b900437fec 100644 --- 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 index 4b604d55613..0e59df8814c 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -256,13 +256,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( @@ -278,7 +279,8 @@ 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...") @@ -286,6 +288,7 @@ def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10): 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( @@ -301,7 +304,8 @@ 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...") @@ -309,6 +313,7 @@ def driveOnHeading(self, dist=0.15, speed=0.025, time_allowance=10): 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(