mpc_local_planner: use transformPose in transformGlobalPlan

Switch to different method as it provides exception handling.

Author: dkuenster

mpc_local_planner/CMakeLists.txt

@@ -16,6 +16,8 @@ find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(nav2_core REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util)
+find_package(nav_2d_utils)
 find_package(costmap_converter REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -106,6 +108,8 @@ set(ament_dependencies
     builtin_interfaces
     nav2_core
     nav2_costmap_2d
+    nav2_util
+    nav_2d_utils
     costmap_converter
     rclcpp
     geometry_msgs

mpc_local_planner/package.xml

@@ -26,6 +26,7 @@
   <depend>nav2_core</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_util</depend>
+  <depend>nav_2d_utils</depend>
   <depend>costmap_converter</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>

mpc_local_planner/src/mpc_local_planner_ros.cpp

@@ -20,6 +20,7 @@
  *  Authors: Christoph Rösmann
  *********************************************************************/
 
+#include <nav_2d_utils/tf_help.hpp>
 #include <mpc_local_planner/mpc_local_planner_ros.h>
 
 #include <mpc_local_planner/utils/math_utils.h>
@@ -610,7 +611,10 @@ bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const st
 
         // let's get the pose of the robot in the frame of the plan
         geometry_msgs::msg::PoseStamped robot_pose;
-        tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);
+
+        rclcpp::Duration transform_tolerance = rclcpp::Duration::from_seconds(0.5);
+
+        nav_2d_utils::transformPose(_tf, plan_pose.header.frame_id, global_pose, robot_pose, transform_tolerance);
 
         // we'll discard points on the plan that are outside the local costmap
         double dist_threshold =