base_local_planner代码解析之二：trajectory_planner_ros.cpp

ROS

heading_lookahead
heading_scoring

可参考：https://blog.csdn.net/Neo11111/article/details/104660830记录对TrajectoryPlannerROS类的阅读和理解。
https://blog.csdn.net/Neo11111/article/details/105320624ROS局部规划器中的轨迹模拟策略-DWA使用与否的差别

goal_function.cpp服务于该cpp。

1. void TrajectoryPlannerROS::initialize(
std::string name,
tf2_ros::Buffer* tf,
costmap_2d::Costmap2DROS* costmap_ros)

costmap_ros（格式为Costmap2DROS：ROS的地图封装类，它整合了静态层、障碍层、膨胀层地图）来初始化本地规划器用到的代价地图。

if(controller_frequency > 0)
    sim_period_ = 1.0 / controller_frequency;
else
{
    ROS_WARN("A controller_frequency less than 0 has been set. Ignoring the parameter, assuming a rate of 20Hz");
    sim_period_ = 0.05;
}




if(meter_scoring) {
      //if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
      double resolution = costmap_->getResolution();
      goal_distance_bias *= resolution;
      path_distance_bias *= resolution;
      occdist_scale *= resolution;
    } 
else {
    ROS_WARN("Trajectory Rollout planner initialized with param meter_scoring set to false. Set it to true to make your settings robust against changes of costmap resolution.");
}




private_nh.param("max_rotational_vel", max_rotational_vel, 1.0);
  max_vel_th_ = max_rotational_vel;
  min_vel_th_ = -1.0 * max_rotational_vel;




if(private_nh.getParam("backup_vel", backup_vel))
        ROS_WARN("The backup_vel parameter has been deprecated in favor of the escape_vel parameter. To switch, just change the parameter name in your configuration files.");



  private_nh.param("world_model", world_model_type, std::string("costmap"));
  private_nh.param("dwa", dwa, true);
  private_nh.param("heading_scoring", heading_scoring, false);
  private_nh.param("heading_scoring_timestep", heading_scoring_timestep, 0.8);


ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");//在初始化world_model_时，用的是CostmapModel类(A class that implements the WorldModel interface to provide grid based collision checks for the trajectory controller using the costmap.)，它是WorldModel的派生类。

//创建TrajectoryPlanner类实例，它是TrajectoryPlannerROS类的成员，并开启动态参数配置服务。
tc_ = new TrajectoryPlanner(*******);
dsrv_ = new dynamic_reconfigure::Server<BaseLocalPlannerConfig>(private_nh);
dynamic_reconfigure::Server<BaseLocalPlannerConfig>::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2);
 dsrv_->setCallback(cb);

2. 传入全局规划 TrajectoryPlannerROS::setPlan | 用全局规划结果给本地规划器赋值参数
Movebase通过调用这个函数传入先前针对当前位置和目标点间规划好的全局路径，

3. 核心函数 TrajectoryPlannerROS::computeVelocityCommands | 一步速度计算
该函数在Movebase的executeCycle函数中被调用，executeCycle函数本身是被循环执行的，所以能够不断进行局部速度规划，从而获得连续的速度指令，控制机器人行动。

首先，获取global系的当前位姿（使用从底盘到global的转换），它可以用来判断是否行进到目标点。

并将全局规划结果global_plan_从地图系转换到global系，得到transformed_plan，这里调用的transformGlobalPlan函数来自goal_functions.cpp，这个文件中定义了一些辅助函数。transformGlobalPlan函数除了通过tf完成坐标转换，还对转换后的路径点做了一些筛选处理，与主体关系不大，这里略过。

这样，得到了global系的当前位姿和全局规划transformed_plan。

prune_plan_是初始化函数中从参数服务器上下载的值，这里对它进行判断，看是否要“修剪”全局规划，“修剪”是指在机器人前进的过程中，将一定阈值外的走过的路径点从global_plan_和transformed_plan中去掉。该函数同样定义在goal_functions中。

4. bool stopWithAccLimits(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, geometry_msgs::Twist& cmd_vel)
@brief Stop the robot taking into account acceleration limits以最大加速度“刹车”

double vx = sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) - acc_lim_x_ * sim_period_));
double vy = sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) - acc_lim_y_ * sim_period_));

double vel_yaw = tf2::getYaw(robot_vel.pose.orientation);
double vth = sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim_theta_ * sim_period_));

5. bool rotateToGoal(const geometry_msgs::PoseStamped& global_pose, const geometry_msgs::PoseStamped& robot_vel, double goal_th, geometry_msgs::Twist& cmd_vel);
* @brief Once a goal position is reached... rotate to the goal orientation

//参考下面，看看min_in_place_vel_th_怎么设

 double v_theta_samp = ang_diff > 0.0 ? std::min(max_vel_th_,
    std::max(min_in_place_vel_th_, ang_diff)) : std::max(min_vel_th_,
    std::min(-1.0 * min_in_place_vel_th_, ang_diff));