Improvements in getCostsCallback (#4767)

tonynajjar · reinzor · SteveMacenski · web-flow · commit d6b02ac407a2 · 2024-12-09T10:36:55.000-08:00
* 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 <rein.appeldoorn@nobleo.nl> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * 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 <rein.appeldoorn@nobleo.nl> * processed comments Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> * comments Steve Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> --------- Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Improvements in RemoveInCollisionGoals and adjacent features (#4676) * improvements Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * ament_uncrustify Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix building Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fixes Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add stamp Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Correct paper name for graceful controller Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * 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 <tiwa@reindeere.ca> * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> --------- Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Adding disengagement threshold to rotation shim controller (#4699) * adding disengagement threshold to rotation shim controller Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * change default to 22.5 deg Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Switch nav2_waypoint_follower to modern CMake idioms. (#4648) Signed-off-by: Chris Lalancette <clalancette@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Feat/migrate gps nav2 system test (#4682) * include missing docking station parameters Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * fix crach RL Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * lintering Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * Change naming Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * update submodule Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * minor changes Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * fix issue with caching Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * fix issue with caching, increase version number Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> * Pin git ref via sha to bust underlay workspace cache --------- Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Co-authored-by: ruffsl <roxfoxpox@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix vel smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server unit testing Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing last bits Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1 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 <antoine.gennart@quimesis.be> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Improve Docking panel (#4717) * Added load and save panel Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improved dock_panel state machine Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added loading dock plugins log Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Redo UI Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update tooltips Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix null-dereference Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [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 <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [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 <adi.vardi@enway.ai> * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Use single quotes Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Remove nav2_loopback_sim dependency on transforms3d. (#4738) The package never uses it, so don't declare it. Signed-off-by: Chris Lalancette <clalancette@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix missing dependency on nav2_costmap_2d (#4742) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * linting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * 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 <redvinaa@gmail.com> * Fix battery sub creation bug Signed-off-by: redvinaa <redvinaa@gmail.com> --------- Signed-off-by: redvinaa <redvinaa@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * 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 <dylan.decoeyer@quimesis.be> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * improvements Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * change back to NO_INFORMATION Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "change back to NO_INFORMATION" This reverts commit 9f8c69c. Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add IsStoppedBTNode Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add topic name + reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix comment Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix abs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove log Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add getter functions for raw twist Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove unused code Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use odomsmoother Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reset at success Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * 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 <michael.wake@aosgrp.com.au> * 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 <michael.wake@aosgrp.com.au> * 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 <michael.wake@aosgrp.com.au> * 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 <michael.wake@aosgrp.com.au> * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> --------- Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Added collision detection for docking (#4752) * Added collision detection for docking Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor fixes Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improve collision while undocking and test Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix smoke testing Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Rename dock_collision_threshold Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added docs and params Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Minor changes in README Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Use BT.CPP Tree::sleep (#4761) * Use BT.cpp sleep Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Implement BT Loop Rate Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * move to nav2_behavior_tree Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix lint Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * cache Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix stopped Node Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add tests to odometry_utils Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Simplify namespaced bringups and multirobot sim (#4715) * WIP single robot namespacing working Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com> * Remove manual namespace substitution Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com> * Remove all multirobot specific configs Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com> * Refactor parsing function to common, add for rest of layers Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Fix dwb_critics test Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Add alternative API for costmap construction Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Address review feedback Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Remove remaining usage of `use_namespace` parameter Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Always join with parent namespace Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Use private parameter for parent namespace Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Fix integration test Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Revert "Use private parameter for parent namespace" This reverts commit 0c958dc. Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Revert "Fix integration test" This reverts commit 137d577. Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Remove global map_topic parameter Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> * Simplify Costmap2DROS constructor Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> --------- Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com> Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Make RecoveryNode return Running (#4777) * Add IsStoppedBTNode Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add topic name + reformat Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix comment Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix abs Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove log Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add getter functions for raw twist Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * remove unused code Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * use odomsmoother Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix formatting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * update groot Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * reset at success Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * FIX velocity_threshold_ Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix stopped Node Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Add tests to odometry_utils Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix linting Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Make RecoveryNode return RUNNING Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * PR review Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add halt at the end Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Migrating twist to twiststamped in simulations for recommended default bringups (#4779) * migrating from twist to twiststamped Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix assisted teleop test Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server smoke test Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bust nav2 minimal tb sim cache --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * address comments Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * set response to true Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fail if out of bounds Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> Signed-off-by: Chris Lalancette <clalancette@gmail.com> Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> Signed-off-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: redvinaa <redvinaa@gmail.com> Signed-off-by: Dylan De Coeyer <dylan.decoeyer@quimesis.be> Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Luca Della Vedova <lucadellavr@gmail.com> Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai> Co-authored-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Chris Lalancette <clalancette@gmail.com> Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Co-authored-by: ruffsl <roxfoxpox@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: Daniil Khaninaev <khaninaev@yahoo.com> 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 <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> 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 <lucadv@intrinsic.ai>
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
@@ -55,6 +55,14 @@ void RemoveInCollisionGoals::on_tick()
 BT::NodeStatus RemoveInCollisionGoals::on_completion(
   std::shared_ptr<nav2_msgs::srv::GetCosts::Response> 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
@@ -25,10 +25,10 @@
 #include "utils/test_behavior_tree_fixture.hpp"
 
 
-class RemoveInCollisionGoalsService : public TestService<nav2_msgs::srv::GetCosts>
+class RemoveInCollisionGoalsSucessService : public TestService<nav2_msgs::srv::GetCosts>
 {
 public:
-  RemoveInCollisionGoalsService()
+  RemoveInCollisionGoalsSucessService()
   : TestService("/global_costmap/get_cost_global_costmap")
   {}
 
@@ -40,9 +40,28 @@ class RemoveInCollisionGoalsService : public TestService<nav2_msgs::srv::GetCost
     (void)request_header;
     (void)request;
     response->costs = {100, 50, 5, 254};
+    response->success = true;
   }
 };
 
+class RemoveInCollisionGoalsFailureService : public TestService<nav2_msgs::srv::GetCosts>
+{
+public:
+  RemoveInCollisionGoalsFailureService()
+  : TestService("/local_costmap/get_cost_local_costmap")
+  {}
+
+  virtual void handle_service(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::GetCosts::Request> request,
+    const std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
+  {
+    (void)request_header;
+    (void)request;
+    response->costs = {255, 50, 5, 254};
+    response->success = false;
+  }
+};
 
 class RemoveInCollisionGoalsTestFixture : public ::testing::Test
 {
@@ -86,15 +105,16 @@ class RemoveInCollisionGoalsTestFixture : public ::testing::Test
     delete config_;
     config_ = nullptr;
     node_.reset();
-    server_.reset();
+    success_server_.reset();
     factory_.reset();
   }
 
   void TearDown() override
   {
     tree_.reset();
   }
-  static std::shared_ptr<RemoveInCollisionGoalsService> server_;
+  static std::shared_ptr<RemoveInCollisionGoalsSucessService> success_server_;
+  static std::shared_ptr<RemoveInCollisionGoalsFailureService> 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<RemoveInCollisionGoalsService>
-RemoveInCollisionGoalsTestFixture::server_ = nullptr;
+std::shared_ptr<RemoveInCollisionGoalsSucessService>
+RemoveInCollisionGoalsTestFixture::success_server_ = nullptr;
+std::shared_ptr<RemoveInCollisionGoalsFailureService>
+RemoveInCollisionGoalsTestFixture::failure_server_ = nullptr;
 std::shared_ptr<BT::BehaviorTreeFactory> RemoveInCollisionGoalsTestFixture::factory_ = nullptr;
 std::shared_ptr<BT::Tree> 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<geometry_msgs::msg::PoseStamped> 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"(
+      <root BTCPP_format="4">
+        <BehaviorTree ID="MainTree">
+          <RemoveInCollisionGoals service_name="/local_costmap/get_cost_local_costmap" input_goals="{goals}" output_goals="{goals}" cost_threshold="253"/>
+        </BehaviorTree>
+      </root>)";
+
+  tree_ = std::make_shared<BT::Tree>(factory_->createTreeFromText(xml_txt, config_->blackboard));
+
+  // create new goal and set it on blackboard
+  std::vector<geometry_msgs::msg::PoseStamped> 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<geometry_msgs::msg::PoseStamped> 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<RemoveInCollisionGoalsService>();
-  std::thread server_thread([]() {
-      rclcpp::spin(RemoveInCollisionGoalsTestFixture::server_);
+  RemoveInCollisionGoalsTestFixture::success_server_ =
+    std::make_shared<RemoveInCollisionGoalsSucessService>();
+  std::thread success_server_thread([]() {
+      rclcpp::spin(RemoveInCollisionGoalsTestFixture::success_server_);
+    });
+
+  RemoveInCollisionGoalsTestFixture::failure_server_ =
+    std::make_shared<RemoveInCollisionGoalsFailureService>();
+  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
@@ -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<float>(costmap->getCost(mx, my)));
     }
diff --git 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
+float32[] costs
+bool success
