# doubt regarding the base_local_planner::transformGlobalPlan() function

I am trying to understand the base_local_planner::transformGlobalPlan() function.

As per my understanding, the function takes in the global_plan (generated by the global planner) as an input and updates the transformedPlan variable. It does so by transforming the points in the global_plan to the global_frame (of the local costmap) and then removing the points that lie outside the bounds of the local costmap.

In particular, I am trying to understand the section inside the first while loop - marked by the comments - 'we need to loop to a point on the plan that is within a certain distance of the robot'.

In this section, we are looping over the points in the global_plan until sq_dist <= sq_dist_threshold. Shouldn't this cause the first while loop to break immediately because the first point in the global_plan would be the one that is closest to the bot?

bool transformGlobalPlan(
const tf2_ros::Buffer& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const geometry_msgs::PoseStamped& global_pose,
const costmap_2d::Costmap2D& costmap,
const std::string& global_frame,
std::vector<geometry_msgs::PoseStamped>& transformed_plan){
transformed_plan.clear();

if (global_plan.empty()) {
ROS_ERROR("Received plan with zero length");
return false;
}

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, ros::Time(),

//let's get the pose of the robot in the frame of the plan
geometry_msgs::PoseStamped robot_pose;

//we'll discard points on the plan that are outside the local costmap
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);

unsigned int i = 0;
double sq_dist_threshold = dist_threshold * dist_threshold;
double sq_dist = 0;

//we need to loop to a point on the plan that is within a certain distance of the robot
while(i < (unsigned int)global_plan.size()) {
double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;
if (sq_dist <= sq_dist_threshold) {
break;
}
++i;
}

//now we'll transform until points are outside of our distance threshold
while(i < (unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
const geometry_msgs::PoseStamped& pose = global_plan[i];

double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
sq_dist = x_diff * x_diff + y_diff * y_diff;

++i;
}
}
catch(tf2::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
return false;
}
catch(tf2::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
return false;
}
catch(tf2::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
if (!global_plan.empty())
ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());

return false;
}

return true;
}


Shouldn't this cause the first while loop to break immediately because the first point in the global_plan would be the one that is closest to the bot?
That looks like the intent to me, but I can't tell where global_plan is coming from or if it's guaranteed to be sorted by distance. Not quite sure what the point is here, but if you had something like a figure 8 track the position that's closest to you may not necessarily be the one that's next in order if the points are populated along a path.