transformGlobalPlan功能是根据本地代价地图，从全局路径global_plan截取一段，并将路径点从/map坐标系转换到/odom坐标系，生成结果放在参数transformed_plan。
在图1，AD这条红色曲线是makePlan规划出的全局路径global_plan，中间有145个路径点。[0]A(1.17739, 1.75393)是起始，[144]D(3.13889, 2.50000)是终点，[19]B(0.95596, 2.18709)、C分别在曲线上，它们都是map坐标系。
点R是机器人当前在map坐标系下坐标。
transformGlobalPlan可分为两个阶段。
- 从起始点开始，找到第一个落在本地代价地图内的点。图1中，由于第一个点A就在落在本地代价地图内，所以i=0，
- 从i点开始，找到第一个落在本地代价地图外的点，把当中代价地图内的点放入transformed_plan。放入时会由map坐标系转为odom坐标系。示例中B是第一个落在本地代价图外的点，于是第二阶段后，i=19，而#0到#18的点则放入transformed_plan，当然，在放入时要由map坐标系转为odom坐标系。
怎么判断路径点A是否要放入transformed_plan？——把机器人中心放到/map坐标系下，记为robot_pose。如果A和robot_pose之间路径小于1.5米（本地代价地图半径），则放入。
过程中路径点只是坐标系变换，不会发生插补。
在转换时，是不是已经走过的全局路径点，也会落在transformed_plan？——会。只要那些点和robot_pose距离小于1.5米。接下如果出现一次本地规划得不到轨迹，那MoveBase会调用makePlan，那些走过的全局路径点自然也没了。
一、官方源码
- [IN]tf。用于获取map-->odom的坐标系变换tf。
- [IN]global_plan。makePlan生成的全局路径。
- [IN]global_pose。机器人在/odom坐标系下的当前位姿。即图1中的current_pose_。
- [IN]costmap。本地代价地图。
- [IN]global_frame。路径点要转换到的坐标系。示例“odom”。
- [OUT]transformed_plan。存储截取出的路径。
<libros>/base_local_planner/src/goal_functions.cpp
------
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(),
plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id, ros::Duration(0.5));
//let's get the pose of the robot in the frame of the plan
geometry_msgs::PoseStamped robot_pose;
tf.transform(global_pose, robot_pose, plan_pose.header.frame_id);plan_to_global_transform是map-->odom的tf，global_pose是odom坐标系下的，robot_pose就是global_pose对应map坐标系下的。
//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;
}第一阶段，从起始点开始，找到第一个落在本地代价地图内的点。这个点记为i。
geometry_msgs::PoseStamped newer_pose;
//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];
tf2::doTransform(pose, newer_pose, plan_to_global_transform);
transformed_plan.push_back(newer_pose);
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;
}
第二阶段，从i点开始，找到第一个落在本地代价地图外的点，把当中代价地图内的点放入transformed_plan。放入时会由map坐标系转为odom坐标系。
}
catch(tf2::LookupException& ex) {
...
}
return true;
}
二、修改源码
回到图1，按正确逻辑，指导路径应该是从R附近开始到终点D这段曲线。官方代码算出的则是AB(准确说，终点是B前面那个点)，这已是个错误的指导路径。
解决思路是要在第一阶段找到正确i点。以下是我用方法。
- 以机器人当前位置R点为中心，扩展出一个正方形。
- 从A开始，找出第一个在正方形中的点，这个点作为第一阶段的结果i。图1中是点C附近。
- 如果没找到正方形中的点，认为这条全局路径不适合的，需要调用makePlan重新规划全局路径。