Throw ControllerTFError when end pose cannot be transformed inside isGoalReached (ros-navigation#6285)

Signed-off-by: Alireza Moayyedi <alireza.moayyedi@nobleo.nl>

Author: alireza-moayyedi

nav2_controller/src/controller_server.cpp

@@ -811,9 +811,12 @@ bool ControllerServer::isGoalReached()
 
   geometry_msgs::msg::PoseStamped transformed_end_pose;
   rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
-  nav_2d_utils::transformPose(
+  if(!nav_2d_utils::transformPose(
     costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
-    end_pose_, transformed_end_pose, tolerance);
+    end_pose_, transformed_end_pose, tolerance))
+  {
+    throw nav2_core::ControllerTFError("Failed to transform end pose to global frame");
+  }
 
   return goal_checkers_[current_goal_checker_]->isGoalReached(
     pose.pose, transformed_end_pose.pose,

nav2_controller/test/CMakeLists.txt

@@ -1,10 +1,10 @@
 # Test dynamic parameters
-ament_add_gtest(test_dynamic_parameters
-  test_dynamic_parameters.cpp
+ament_add_gtest(test_controller_server
+  test_controller_server.cpp
 )
-ament_target_dependencies(test_dynamic_parameters
+ament_target_dependencies(test_controller_server
   ${dependencies}
 )
-target_link_libraries(test_dynamic_parameters
+target_link_libraries(test_controller_server
   ${library_name}
 )

…troller/test/test_dynamic_parameters.cpp …ntroller/test/test_controller_server.cppnav2_controller/test/test_dynamic_parameters.cpp renamed to nav2_controller/test/test_controller_server.cpp nav2_controller/test/test_dynamic_parameters.cpp renamed to nav2_controller/test/test_controller_server.cpp

@@ -18,10 +18,14 @@
 #include <string>
 #include <vector>
 
+#include "geometry_msgs/msg/transform_stamped.hpp"
 #include "gtest/gtest.h"
-#include "nav2_util/lifecycle_node.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_controller/controller_server.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+#include "nav2_util/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.hpp"
 
 class ControllerShim : public nav2_controller::ControllerServer
 {
@@ -59,7 +63,7 @@ class RclCppFixture
 };
 RclCppFixture g_rclcppfixture;
 
-TEST(WPTest, test_dynamic_parameters)
+TEST(ControllerServerTest, test_dynamic_parameters)
 {
   auto controller = std::make_shared<ControllerShim>();
   controller->setDynamicCallback();
@@ -86,3 +90,39 @@ TEST(WPTest, test_dynamic_parameters)
   EXPECT_EQ(controller->get_parameter("min_theta_velocity_threshold").as_double(), 100.0);
   EXPECT_EQ(controller->get_parameter("failure_tolerance").as_double(), 5.0);
 }
+
+class GoalReachTestController : public nav2_controller::ControllerServer
+{
+public:
+  using nav2_controller::ControllerServer::ControllerServer;
+
+  void setEndPoseFrame(const std::string & frame) {end_pose_.header.frame_id = frame;}
+  bool callIsGoalReached() {return isGoalReached();}
+  tf2_ros::Buffer & getTfBuffer() {return *costmap_ros_->getTfBuffer();}
+};
+
+TEST(ControllerServerTest, IsGoalReachedThrowsOnTfFailure)
+{
+  rclcpp::NodeOptions options;
+  options.parameter_overrides({
+    rclcpp::Parameter("progress_checker_plugins", std::vector<std::string>{}),
+    rclcpp::Parameter("goal_checker_plugins", std::vector<std::string>{}),
+    rclcpp::Parameter("controller_plugins", std::vector<std::string>{}),
+  });
+
+  auto server = std::make_shared<GoalReachTestController>(options);
+  ASSERT_EQ(
+    server->configure().id(),
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
+  geometry_msgs::msg::TransformStamped tf_msg;
+  tf_msg.header.stamp = server->now();
+  tf_msg.header.frame_id = "map";
+  tf_msg.child_frame_id = "base_link";
+  tf_msg.transform.rotation.w = 1.0;
+  server->getTfBuffer().setTransform(tf_msg, "test", true);
+
+  server->setEndPoseFrame("nonexistent_frame_xyz");
+  EXPECT_THROW(server->callIsGoalReached(), nav2_core::ControllerTFError);
+  server->cleanup();
+}