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 d6f9ee662d0..78e6c2e35df 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; + } + Goals 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/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 dbb4889298d..626d0b1b15d 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 std::vector 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[2], 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_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5413fd3d2c1..a35a1786bb4 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) { 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/srv/GetCosts.srv b/nav2_msgs/srv/GetCosts.srv index 55ebad4f0a2..c5ca48a3099 100644 --- a/nav2_msgs/srv/GetCosts.srv +++ b/nav2_msgs/srv/GetCosts.srv @@ -3,4 +3,5 @@ bool use_footprint geometry_msgs/PoseStamped[] poses --- -float32[] costs \ No newline at end of file +float32[] costs +bool success \ No newline at end of file