From 3ff2d2ab38039031bf6d3a88a712171a004451e3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 9 Dec 2024 19:36:55 +0100 Subject: [PATCH] Improvements in getCostsCallback (#4767) * fix(simple-action-server): info log instead of warn on cancel (#4684) Cancelling a goal is nominal behavior and therefore it should not log warning. Signed-off-by: Rein Appeldoorn Signed-off-by: Tony Najjar * feat(controller-server): `publish_zero_velocity` parameter (#4675) * feat(controller-server): `publish_zero_velocity` parameter For optionally publishing a zero velocity command reference on goal exit. Publishing a zero velocity is not desired when we are following consecutive path segments that end with a velocity. Signed-off-by: Rein Appeldoorn * processed comments Signed-off-by: Rein Appeldoorn * comments Steve Signed-off-by: Rein Appeldoorn --------- Signed-off-by: Rein Appeldoorn Signed-off-by: Tony Najjar * Improvements in RemoveInCollisionGoals and adjacent features (#4676) * improvements Signed-off-by: Tony Najjar * ament_uncrustify Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * fix building Signed-off-by: Tony Najjar * fixes Signed-off-by: Tony Najjar * add stamp Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * Correct paper name for graceful controller Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo --------- Signed-off-by: Tiwa Ojo Signed-off-by: Tony Najjar * Adding disengagement threshold to rotation shim controller (#4699) * adding disengagement threshold to rotation shim controller Signed-off-by: Steve Macenski * change default to 22.5 deg Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * Switch nav2_waypoint_follower to modern CMake idioms. (#4648) Signed-off-by: Chris Lalancette Signed-off-by: Tony Najjar * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * Feat/migrate gps nav2 system test (#4682) * include missing docking station parameters Signed-off-by: stevedan * fix crach RL Signed-off-by: stevedan * lintering Signed-off-by: stevedan * Change naming Signed-off-by: stevedan * update submodule Signed-off-by: stevedan * minor changes Signed-off-by: stevedan * fix issue with caching Signed-off-by: stevedan * fix issue with caching, increase version number Signed-off-by: stevedan * Pin git ref via sha to bust underlay workspace cache --------- Signed-off-by: stevedan Co-authored-by: ruffsl Signed-off-by: Tony Najjar * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas * adding support for rest of servers + review comments Signed-off-by: Steve Macenski * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski * fix vel smoother unit tests Signed-off-by: Steve Macenski * fixing docking server unit testing Signed-off-by: Steve Macenski * fixing last bits Signed-off-by: Steve Macenski --------- Signed-off-by: Kemal Bektas Signed-off-by: Steve Macenski Co-authored-by: Kemal Bektas Signed-off-by: Tony Najjar * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1f9b8233e86ed54dfbe615b1ba56b51b6d added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart Signed-off-by: Tony Najjar * Improve Docking panel (#4717) * Added load and save panel Signed-off-by: Alberto Tudela * Improved dock_panel state machine Signed-off-by: Alberto Tudela * Added loading dock plugins log Signed-off-by: Alberto Tudela * Redo UI Signed-off-by: Alberto Tudela * Update tooltips Signed-off-by: Alberto Tudela * Fix null-dereference Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela Signed-off-by: Tony Najjar * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev Signed-off-by: Tony Najjar * [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart Co-authored-by: agennart Signed-off-by: Tony Najjar * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi * [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t. This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data. In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS) Signed-off-by: Adi Vardi * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi * Use single quotes Signed-off-by: Adi Vardi --------- Signed-off-by: Adi Vardi Signed-off-by: Tony Najjar * Remove nav2_loopback_sim dependency on transforms3d. (#4738) The package never uses it, so don't declare it. Signed-off-by: Chris Lalancette Signed-off-by: Tony Najjar * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Tony Najjar * Fix missing dependency on nav2_costmap_2d (#4742) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Tony Najjar * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski * linting Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Signed-off-by: Tony Najjar * Pass IDLE to on_tick, use that for initialize condition (#4744) * Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa * Fix battery sub creation bug Signed-off-by: redvinaa --------- Signed-off-by: redvinaa Signed-off-by: Tony Najjar * nav2_costmap_2d: add missing default_value_ copy in Costmap2D operator= (#4753) default_value_ is an attribute of the Costmap2D class and should be copied along with the other attributes. Signed-off-by: Dylan De Coeyer Signed-off-by: Tony Najjar * improvements Signed-off-by: Tony Najjar * change back to NO_INFORMATION Signed-off-by: Tony Najjar * Revert "change back to NO_INFORMATION" This reverts commit 9f8c69cfb42a983efa535ccbe7a101a813616115. Signed-off-by: Tony Najjar * Add IsStoppedBTNode Signed-off-by: Tony Najjar * add topic name + reformat Signed-off-by: Tony Najjar * fix comment Signed-off-by: Tony Najjar * fix abs Signed-off-by: Tony Najjar * remove log Signed-off-by: Tony Najjar * add getter functions for raw twist Signed-off-by: Tony Najjar * remove unused code Signed-off-by: Tony Najjar * use odomsmoother Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * update groot Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * reset at success Signed-off-by: Tony Najjar * mppi parameters_handler: Improve verbose handling (#4704) (#4711) * mppi parameters_handler: Improve verbose handling (#4704) The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake * Address review comments. (#4704) * remove comments. * Use RCLCPP_DEBUG instead of INFO for low level messages. * Add test for trying to access parameters that are not declared. Signed-off-by: Mike Wake * mppi parameters_handler: Improve static/dynamic/not defined logging (#4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted. Signed-off-by: Mike Wake * mppi parameters_handler: populate SetParametersResult (#4704) Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult with the reasons for unsuccessful parameter setting, so that it may be propogated back to a set parameter client. The mechanism provides feedback when attempting to set undefined parameters, static parameters and could be used to validate dynamic parameters and provide a reason for rejection. NOTE: This changes public interface of ParametersHandler class. s/setDynamicParamCallback/setParamCallback s/addDynamicParamCallback/addParamCallback which takes a callback function that is able to populate a rcl_interfaces::msg::SetParametersResult. In order to indicate an unsuccessful parameter change and the reason, callback functions should append a "\n" to the reason before appending to if it is not empty. Signed-off-by: Mike Wake * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake --------- Signed-off-by: Mike Wake Signed-off-by: Tony Najjar * Added collision detection for docking (#4752) * Added collision detection for docking Signed-off-by: Alberto Tudela * Minor fixes Signed-off-by: Alberto Tudela * Improve collision while undocking and test Signed-off-by: Alberto Tudela * Fix smoke testing Signed-off-by: Alberto Tudela * Rename dock_collision_threshold Signed-off-by: Alberto Tudela * Added docs and params Signed-off-by: Alberto Tudela * Minor changes in README Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela Signed-off-by: Tony Najjar * Use BT.CPP Tree::sleep (#4761) * Use BT.cpp sleep Signed-off-by: Tony Najjar * Implement BT Loop Rate Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * Fix formatting Signed-off-by: Tony Najjar * move to nav2_behavior_tree Signed-off-by: Tony Najjar * fix Signed-off-by: Tony Najjar * fix lint Signed-off-by: Tony Najjar * cache Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * FIX velocity_threshold_ Signed-off-by: Tony Najjar * Fix stopped Node Signed-off-by: Tony Najjar * Add tests to odometry_utils Signed-off-by: Tony Najjar * fix linting Signed-off-by: Tony Najjar * Simplify namespaced bringups and multirobot sim (#4715) * WIP single robot namespacing working Signed-off-by: Luca Della Vedova * Remove manual namespace substitution Signed-off-by: Luca Della Vedova * Remove all multirobot specific configs Signed-off-by: Luca Della Vedova * Refactor parsing function to common, add for rest of layers Signed-off-by: Luca Della Vedova * Fix dwb_critics test Signed-off-by: Luca Della Vedova * Add alternative API for costmap construction Signed-off-by: Luca Della Vedova * Address review feedback Signed-off-by: Luca Della Vedova * Remove remaining usage of `use_namespace` parameter Signed-off-by: Luca Della Vedova * Always join with parent namespace Signed-off-by: Luca Della Vedova * Use private parameter for parent namespace Signed-off-by: Luca Della Vedova * Fix integration test Signed-off-by: Luca Della Vedova * Revert "Use private parameter for parent namespace" This reverts commit 0c958dc994f5e618fef2471c0851a2bc3b19b1d5. Signed-off-by: Luca Della Vedova * Revert "Fix integration test" This reverts commit 137d57759b700f00c7548b997abcc8dc36d9cca9. Signed-off-by: Luca Della Vedova * Remove global map_topic parameter Signed-off-by: Luca Della Vedova * Simplify Costmap2DROS constructor Signed-off-by: Luca Della Vedova --------- Signed-off-by: Luca Della Vedova Signed-off-by: Luca Della Vedova Signed-off-by: Tony Najjar * Make RecoveryNode return Running (#4777) * Add IsStoppedBTNode Signed-off-by: Tony Najjar * add topic name + reformat Signed-off-by: Tony Najjar * fix comment Signed-off-by: Tony Najjar * fix abs Signed-off-by: Tony Najjar * remove log Signed-off-by: Tony Najjar * add getter functions for raw twist Signed-off-by: Tony Najjar * remove unused code Signed-off-by: Tony Najjar * use odomsmoother Signed-off-by: Tony Najjar * fix formatting Signed-off-by: Tony Najjar * update groot Signed-off-by: Tony Najjar * Add test Signed-off-by: Tony Najjar * reset at success Signed-off-by: Tony Najjar * FIX velocity_threshold_ Signed-off-by: Tony Najjar * Fix stopped Node Signed-off-by: Tony Najjar * Add tests to odometry_utils Signed-off-by: Tony Najjar * fix linting Signed-off-by: Tony Najjar * Make RecoveryNode return RUNNING Signed-off-by: Tony Najjar * PR review Signed-off-by: Tony Najjar * add halt at the end Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * Migrating twist to twiststamped in simulations for recommended default bringups (#4779) * migrating from twist to twiststamped Signed-off-by: Steve Macenski * bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski * fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski * fix assisted teleop test Signed-off-by: Steve Macenski * fixing docking server smoke test Signed-off-by: Steve Macenski * bust nav2 minimal tb sim cache --------- Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar * address comments Signed-off-by: Tony Najjar * set response to true Signed-off-by: Tony Najjar * fix test Signed-off-by: Tony Najjar * fail if out of bounds Signed-off-by: Tony Najjar --------- Signed-off-by: Rein Appeldoorn Signed-off-by: Tony Najjar Signed-off-by: Steve Macenski Signed-off-by: Tiwa Ojo Signed-off-by: Chris Lalancette Signed-off-by: stevedan Signed-off-by: Kemal Bektas Signed-off-by: Alberto Tudela Signed-off-by: Daniil Khaninaev Signed-off-by: agennart Signed-off-by: Adi Vardi Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy Signed-off-by: redvinaa Signed-off-by: Dylan De Coeyer Signed-off-by: Mike Wake Signed-off-by: Luca Della Vedova Signed-off-by: Luca Della Vedova Co-authored-by: Rein Appeldoorn Co-authored-by: Steve Macenski Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Chris Lalancette Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: ruffsl Co-authored-by: Kemal Bektas Co-authored-by: Saitama Co-authored-by: agennart Co-authored-by: Alberto Tudela Co-authored-by: Daniil Khaninaev Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Vince Reda <60265874+redvinaa@users.noreply.github.com> Co-authored-by: DylanDeCoeyer-Quimesis <102609575+DylanDeCoeyer-Quimesis@users.noreply.github.com> Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com> Co-authored-by: Luca Della Vedova Signed-off-by: Joseph Duchesne --- .../remove_in_collision_goals_action.cpp | 8 ++ .../test_remove_in_collision_goals_action.cpp | 107 +++++++++++++++--- nav2_costmap_2d/src/costmap_2d_ros.cpp | 22 ++-- nav2_msgs/srv/GetCosts.srv | 3 +- 4 files changed, 118 insertions(+), 22 deletions(-) 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