// prune global plan to cut off parts of the past (spatially before the robot)pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance);
// 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
&&fabs(delta_orient)< cfg_.goal_tolerance.yaw_goal_tolerance
&&(!cfg_.goal_tolerance.complete_global_plan || via_points_.size()==0)){
goal_reached_ =true;returntrue;}
5-检测是否轨迹有异常并进入备份模式同时设置一些参数
// check if we should enter any backup mode and apply settingsconfigureBackupModes(transformed_plan, goal_idx);
6-检测截取的transformed_plan是否为空
// Return false if the transformed global plan is emptyif(transformed_plan.empty()){
ROS_WARN("Transformed plan is empty. Cannot determine a local plan.");returnfalse;}
// Get current goal point (last point of the transformed plan)
tf::Stamped<tf::Pose> goal_point;
tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
robot_goal_.x()= goal_point.getOrigin().getX();
robot_goal_.y()= goal_point.getOrigin().getY();if(cfg_.trajectory.global_plan_overwrite_orientation){
robot_goal_.theta()=estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global);// overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)
transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta());}else{
robot_goal_.theta()= tf::getYaw(goal_point.getRotation());}
8-一般发生在目标位置基本不变或已经到达,但是角度不对,发生的纯旋转运动
// overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory)if(transformed_plan.size()==1)// plan only contains the goal{
transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped());// insert start (not yet initialized)、、}
tf::poseTFToMsg(robot_pose, transformed_plan.front().pose);// update start;
9-和障碍物有关的操作,这里暂时跳过
// clear currently existing obstacles
obstacles_.clear();// Update obstacle container with costmap information or polygons provided by a costmap_converter pluginif(costmap_converter_)updateObstacleContainerWithCostmapConverter();elseupdateObstacleContainerWithCostmap();// also consider custom obstacles (must be called after other updates, since the container is not cleared)updateObstacleContainerWithCustomObstacles();
10-重头戏,计算优化后的局部规划结果,如果没有成功,则巴拉巴拉记录一番。
// Do not allow config changes during the following optimization step
boost::mutex::scoped_lock cfg_lock(cfg_.configMutex());// Now perform the actual planning// bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line initbool success = planner_->plan(transformed_plan,&robot_vel_, cfg_.goal_tolerance.free_goal_vel);if(!success){
planner_->clearPlanner();// force reinitialization for next timeROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting.");++no_infeasible_plans_;// increase number of infeasible solutions in a row
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel;returnfalse;}
// Check feasibility (but within the first few states only)if(cfg_.robot.is_footprint_dynamic){
// Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices.
footprint_spec_ = costmap_ros_->getRobotFootprint();
costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius);}bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);if(!feasible){
cmd_vel.linear.x =0;
cmd_vel.linear.y =0;
cmd_vel.angular.z =0;// now we reset everything to start again with the initialization of new trajectories.
planner_->clearPlanner();ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...");++no_infeasible_plans_;// increase number of infeasible solutions in a row
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel;returnfalse;}
// Get the velocity command for this sampling intervalif(!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.trajectory.control_look_ahead_poses)){
planner_->clearPlanner();ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner...");++no_infeasible_plans_;// increase number of infeasible solutions in a row
time_last_infeasible_plan_ = ros::Time::now();
last_cmd_ = cmd_vel;returnfalse;}
13-当优化后的局部规划速度超过了最大速度,则限制速度。
// Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).saturateVelocity(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y,
cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);
14-对于carlike-robot(阿卡曼模型)的小车,可以使用控制角度代替角速度
// convert rot-vel to steering angle if desired (carlike robot).// The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint// and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself.if(cfg_.robot.cmd_angle_instead_rotvel){
cmd_vel.angular.z =convertTransRotVelToSteeringAngle(cmd_vel.linear.x, cmd_vel.angular.z, cfg_.robot.wheelbase,0.95*cfg_.robot.min_turning_radius);if(!std::isfinite(cmd_vel.angular.z)){
cmd_vel.linear.x = cmd_vel.linear.y = cmd_vel.angular.z =0;
last_cmd_ = cmd_vel;
planner_->clearPlanner();ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...");++no_infeasible_plans_;// increase number of infeasible solutions in a row
time_last_infeasible_plan_ = ros::Time::now();returnfalse;}}
15-万事大吉,重置记录一些信息,可视化一下
// a feasible solution should be found, reset counter
no_infeasible_plans_ =0;// store last command (for recovery analysis etc.)
last_cmd_ = cmd_vel;// Now visualize everything
planner_->visualize();
visualization_->publishObstacles(obstacles_);
visualization_->publishViaPoints(via_points_);
visualization_->publishGlobalPlan(global_plan_);returntrue;