From 8bf95c46c45c25abdd975e6801c437c79b9b0641 Mon Sep 17 00:00:00 2001 From: Pavlo Kolomiiets Date: Fri, 16 Mar 2018 11:12:31 +0200 Subject: [PATCH] goal_functions: Replace redundant advanced TF transforms' calls. --- base_local_planner/src/goal_functions.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/base_local_planner/src/goal_functions.cpp b/base_local_planner/src/goal_functions.cpp index 97e4223632..1ba3e1dd97 100644 --- a/base_local_planner/src/goal_functions.cpp +++ b/base_local_planner/src/goal_functions.cpp @@ -111,8 +111,7 @@ namespace base_local_planner { const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; try { // get plan_to_global_transform from plan frame to global_frame - geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, time, - plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5)); + geometry_msgs::TransformStamped plan_to_global_transform = tf.lookupTransform(global_frame, plan_pose.header.frame_id, time, ros::Duration(0.5)); //let's get the pose of the robot in the frame of the plan geometry_msgs::PoseStamped robot_pose; @@ -183,9 +182,7 @@ namespace base_local_planner { const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back(); try{ - geometry_msgs::TransformStamped transform = tf.lookupTransform(global_frame, time, - plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp, - plan_goal_pose.header.frame_id, ros::Duration(0.5)); + geometry_msgs::TransformStamped transform = tf.lookupTransform(global_frame, plan_goal_pose.header.frame_id, time, ros::Duration(0.5)); tf2::doTransform(plan_goal_pose, goal_pose, transform); }