MoveBase::executeCycle() —-> computeVelocityCommands，就是从这个函数开始调用TEB算法，下面转入teb_local_planner_ros.cpp文件中。
开始代码:1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97// check if plugin initialized，省略
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.angular.z = 0;
goal_reached_ = false;
// 修改源码
if(!rotate_to_path_ && new_goal_)
{
double start_angle = tf::getYaw(path_start_orientation_);
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
double robot_angle = tf::getYaw(robot_pose.getRotation());
bool result = pointToPath(start_angle, robot_angle, cmd_vel);
if(result)
{
rotate_to_path_ = true;
return true;
}
return true;
}
// Get robot pose in global frame(map坐标系),转为PoseSE2类型
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
robot_pose_ = PoseSE2(robot_pose);
// Get robot velocity from odom
tf::Stamped<tf::Pose> robot_vel_tf;
odom_helper_.getRobotVel(robot_vel_tf);
robot_vel_.linear.x = robot_vel_tf.getOrigin().getX();
robot_vel_.linear.y = robot_vel_tf.getOrigin().getY();
robot_vel_.angular.z = tf::getYaw(robot_vel_tf.getRotation());
// prune global plan to cut off parts of the past (spatially before the robot)
pruneGlobalPlan(*tf_, robot_pose, global_plan_);
// Transform global plan to the frame of interest (w.r.t. the local costmap)
std::vector<geometry_msgs::PoseStamped> transformed_plan;
int goal_idx;
tf::StampedTransform tf_plan_to_global;
if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist,
transformed_plan, &goal_idx, &tf_plan_to_global))
{
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
// update via-points container 这里是false,不用看了
if (!custom_via_points_active_)
updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);
// check if global goal is reached
tf::Stamped<tf::Pose> global_goal;
tf::poseStampedMsgToTF(global_plan_.back(), global_goal);
global_goal.setData( tf_plan_to_global * global_goal );
double dx = global_goal.getOrigin().getX() - robot_pose_.x();
double dy = global_goal.getOrigin().getY() - robot_pose_.y();
double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - robot_pose_.theta() );
// 是否达到目标位置
if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance)
position_reanched_ = true;
// 修改源码
if(position_reached_)
{
double goal_angle = tf::getYaw(goal_orientation_);
double robot_angle = tf::getYaw(robot_pose.getRotation());
bool result = pointToPath(goal_angle, robot_angle, cmd_vel);
if(result)
{
goal_reached_ = true;
return true;
}
return true;
}
if(!goal_reached_)
{
if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance
&& fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance
&& (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0))
{
goal_reached_ = true;
return true;
}
}
// check if we should enter any backup mode and apply settings
configureBackupModes(transformed_plan, goal_idx);
// Return false if the transformed global plan is empty
if (transformed_plan.empty())
{
ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");
return false;
}
pruneGlobalPlan: 截取全局路径的一部分作为局部规划的初始轨迹，主要是把全局路径中机器人已经经过的位姿删去。取决于参数dist_behind_robot，机器人身后要保留的距离，单位米，默认1，不能太小，尤其不能小于地图的cellsize。 在调用前后，global_plan_常常不发生变化，所以不必太关注。transformGlobalPlan: 根据local costmap地图大小，将此地图内的全局规划器的规划保存到transformed_plan中。transformed_plan所在的坐标系仍然为global_frames
global_plan_对应的就是话题/move_base/TebLocalPlannerROS/global_plan，它的size和成员一般在规划两三次后就会变，size一般是逐步变小，transformed_plan也跟随变化。
transformGlobalPlan函数之后，transformed_plan开始和global_plan_相等，transformed_plan逐渐取global_plan_的后面部分，此时goal_idx == global_plan.size() - 1。 有时候transformed_plan取global_plan_中间部分，此时goal_idx != global_plan.size() - 1
这三者会出现以下几种情况:
global_plan_ size: 49, transformed_plan size:49, goal_idx: 48:transformed_plan是从global_plan第1个点开始(id=0), 二者完全相同，goal_idx对应transformed_plan和global_plan_最后一个点global_plan_ size: 102, transformed_plan size:69, goal_idx: 68:transformed_plan是从global_plan第1个点开始(id=0), 但这次 transformed_plan 取得是 global_plan 前半部分，goal_idx对应transformed_plan最后一个点，global_plan_ size: 49, transformed_plan size:45, goal_idx: 48:transformed_plan是从global_plan第5个点开始(id=4)，取global_plan后半部分，goal_idx对应transformed_plan和global_plan_最后一个点global_plan size: 98, transformed_plan size:69, goal_idx: 73:transformed_plan是从global_plan第6个点开始(id=5), 也就是取中间部分，goal_idx对应transformed_plan最后一个点。goal_idx != transformed_plan.size()-1
goal_idx 是 global_plan 中的id，它对应的点一定和 transformed_plan 最后一个点相同，所以后面才可以赋值给goal_point和robot_goal_