【ROS-Navigation】Base Local Planner局部规划-TrajectoryPlanner源码解读-2

记录学习阅读ROS Navigation源码的理解，本文为Base Local Planner局部规划源码学习记录，以文字总结、绘制结构图说明、代码注释为主。仍在学习过程中，有错误欢迎指正，共同进步。

局部规划器以当前速度为参考，产生一个合理且可达的速度采样范围，确定下一步的速度。那么如何筛选呢？它用采样速度生成相应的仿真路径，借助costmap，从障碍物、与目标的距离、与全局规划路径的距离几个方面对路径成本进行评估，选择最优成本的路径，将它对应的采样速度发布给机器人，控制其运动。若在循环生成前向路径的过程中，前方遇障，无法得到前向的有效路径，那么进入逃逸模式，不断后退、旋转，离开一段距离后再进行前向规划，向前运动。在原地自转时，注意震荡控制，防止机器人左右频繁来回旋转。

【结构示意图】

【相关文件】

• base_local_planner/src/trajectory_planner_ros.cpp
• base_local_planner/src/trajectory_planner.cpp
• base_local_planner/src/map_grid.cpp
• base_local_planner/src/map_cell.cpp
• base_local_planner/src/costmap_model.cpp

本篇记录对trajectory_planner.cpp中定义的TrajectoryPlanner类的阅读和理解

【代码分析】

trajectory_planner.cpp

–目录–

TrajectoryPlanner::updatePlan | 传入全局规划路径

TrajectoryPlanner::findBestPath | 局部规划器核心函数

TrajectoryPlanner::createTrajectories | 生成给定速度范围内所有路径

TrajectoryPlanner::generateTrajectorie | 生成单条路径并返回代价

TrajectoryPlanner::checkTrajectorie 与 scoreTrajectorie | 用给定速度生成单条路径并返回代价

<1> TrajectoryPlanner::updatePlan

Movebase调用全局规划器生成全局路径后，传入TrajectoryPlannerROS封装类，再通过这个函数传入真正的局部规划器TrajectoryPlanner类中，并且将全局路径的最终点最为目标点final_goal。

  void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists){
    global_plan_.resize(new_plan.size());
    for(unsigned int i = 0; i < new_plan.size(); ++i){
      global_plan_[i] = new_plan[i];
    }

    if( global_plan_.size() > 0 ){
      geometry_msgs::PoseStamped& final_goal_pose = global_plan_[ global_plan_.size() - 1 ];
      final_goal_x_ = final_goal_pose.pose.position.x;
      final_goal_y_ = final_goal_pose.pose.position.y;
      final_goal_position_valid_ = true;
    } else {
      final_goal_position_valid_ = false;
    }

compute_dists默认为false，即本地规划器在更新全局plan时，不重新计算path_map_和goal_map_。

    if (compute_dists) {
      //reset the map for new operations
      path_map_.resetPathDist();
      goal_map_.resetPathDist();

      //make sure that we update our path based on the global plan and compute costs
      path_map_.setTargetCells(costmap_, global_plan_);
      goal_map_.setLocalGoal(costmap_, global_plan_);
      ROS_DEBUG("Path/Goal distance computed");
    }
  }

<2> TrajectoryPlanner::findBestPath

局部规划的整个流程体现在findBestPath函数中，它能够在范围内生成下一步的可能路线，选择出最优路径，并返回该路径对应的下一步的速度。

首先记录当前位姿（global系下）、速度，将机器人当前位置的足迹的标志位设置为真。

并且，对path_map_和goal_map_的值重置后调用setTargetCells函数/setLocalGoal函数进行更新。这两个地图是局部规划器中专用的“地图”，即MapGrid类，和costmap的组织形式一样，都以cell为单位，path_map_记录各cell与全局规划路径上的cell之间的距离，goal_map_记录各cell与目标cell之间的距离，再最终计算代价时，将这两个因素纳入考虑，以保证局部规划的结果既贴合全局规划路径、又不致偏离目标。

这两个函数的内容和MapGrid类在下一篇记录。

  Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities){

    //将当前机器人位置和方向转变成float形式的vector
    Eigen::Vector3f pos(global_pose.getOrigin().getX(), global_pose.getOrigin().getY(), tf::getYaw(global_pose.getRotation()));
    Eigen::Vector3f vel(global_vel.getOrigin().getX(), global_vel.getOrigin().getY(), tf::getYaw(global_vel.getRotation()));

    //重置地图，清除所有障碍物信息以及地图内容
    path_map_.resetPathDist();
    goal_map_.resetPathDist();

    //用当前位姿获取机器人footprint
    std::vector<base_local_planner::Position2DInt> footprint_list =
        footprint_helper_.getFootprintCells(
            pos,
            footprint_spec_,
            costmap_,
            true);

    //将初始footprint内的所有cell标记 “within_robot = true”，表示在机器人足迹内
    for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }

    //确保根据全局规划global plan更新路径，并计算代价
    //更新哪些地图上的cell是在全局规划路径上的，target_dist设置为0
    //并且通过它们和其他点的相对位置计算出来地图上所有点的target_dist
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");

接下来，调用createTrajectories函数，传入当前位姿、速度、加速度限制，生成合理速度范围内的轨迹，并进行打分，结果返回至best。

    //找到代价最低的轨迹，输入分别是目前机器人位置，速度以及加速度限制
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
        vel[0], vel[1], vel[2],
        acc_lim_x_, acc_lim_y_, acc_lim_theta_); //生成诸多可能轨迹，选取其中打分最高的。这里也就是最关键的一步。
    ROS_DEBUG("Trajectories created");

    /*
    //If we want to print a ppm file to draw goal dist
    char buf[4096];
    sprintf(buf, "base_local_planner.ppm");
    FILE *fp = fopen(buf, "w");
    if(fp){
      fprintf(fp, "P3\n");
      fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
      fprintf(fp, "255\n");
      for(int j = map_.size_y_ - 1; j >= 0; --j){
        for(unsigned int i = 0; i < map_.size_x_; ++i){
          int g_dist = 255 - int(map_(i, j).goal_dist);
          int p_dist = 255 - int(map_(i, j).path_dist);
          if(g_dist < 0)
            g_dist = 0;
          if(p_dist < 0)
            p_dist = 0;
          fprintf(fp, "%d 0 %d ", g_dist, 0);
        }
        fprintf(fp, "\n");
      }
      fclose(fp);
    }
    */

对返回的结果进行判断，若其代价为负，表示说明所有的路径都不可用；若代价非负，表示找到有效路径，为drive_velocities填充速度后返回。

    if(best.cost_ < 0){
      drive_velocities.setIdentity(); 
    }
    else{
      tf::Vector3 start(best.xv_, best.yv_, 0);
      drive_velocities.setOrigin(start);
      tf::Matrix3x3 matrix;
      matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
      drive_velocities.setBasis(matrix);
    }

    return best; //返回最优轨迹
  }

<3> TrajectoryPlanner::createTrajectories

首先，计算可行的线速度和角速度范围，这里先对最大线速度进行一个限制，即保证速度既不超过预设的最大速度限制，也不超过“起点与目标直线距离/总仿真时间”。

  Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
      double vx, double vy, double vtheta,
      double acc_x, double acc_y, double acc_theta) {
          
    //声明最大/小线速度，最大/小角速度
    double max_vel_x = max_vel_x_, max_vel_theta;
    double min_vel_x, min_vel_theta;

    //如果最终的目标是有效的（全局规划不为空）
    if( final_goal_position_valid_ ){
      //计算当前位置和目标位置之间的距离：final_goal_dist
      double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
      //最大速度：在预设的最大速度和
      max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
    }

继续计算线速度与角速度的上下限，使用的限制是由当前速度在一段时间内，由最大加减速度达到的速度范围，这里进行了一个判断，即是否使用dwa法：

① 使用dwa法，则用的是轨迹前向模拟的周期sim_period_（专用于dwa法计算速度的一个时间间隔）；

② 不使用dwa法，则用的是整段仿真时间sim_time_

在生成范围时注意用预先给定的速度和角速度范围参数进行保护。

    if (dwa_) {
      //使用dwa窗口法，sim_period_是dwa计算最大、最小速度用的时间
      //计算速度、角速度范围，引入加速度限制条件（用sim_period_）
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
    } else {
      //不使用dwa窗口法
      //计算速度、角速度范围，引入加速度限制条件（用sim_time_）
      max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
      min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);

      max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
      min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
    }

接下来根据预设的线速度与角速度的采样数，和上面计算得到的范围，分别计算出采样间隔，并把范围内最小的线速度和角速度作为初始采样速度。不考虑全向机器人的情况，即不能y向移动，故暂不考虑vy上采样。

    double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);//计算速度采样间隔
    double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);//计算角速度采样间隔

    double vx_samp = min_vel_x;//x方向速度采样点
    double vtheta_samp = min_vel_theta;//角速度采样点
    double vy_samp = 0.0;
     

为了迭代比较不同采样速度生成的不同路径的代价，这里声明best_traj和comp_traj并都将其代价初始化为-1

	Trajectory* best_traj = &traj_one;
    best_traj->cost_ = -1.0;//先初始化一个最优路径的代价=-1

    Trajectory* comp_traj = &traj_two; //下面生成的轨迹放到这里，和best_traj作比较，如果生成的轨迹代价更小，就选择它
    comp_traj->cost_ = -1.0;//先初始化一个当前“对比”路径的代价=-1，等会再用generateTrajectory函数生成路径和新的代价存放在里面

    Trajectory* swap = NULL; //用于交换的指针

在机器人没有处于逃逸状态时，开始遍历所有线速度和角速度，调用类内generateTrajectory函数用它们生成轨迹。二重迭代时，外层遍历线速度（正值），内层遍历角速度。在遍历时，单独拎出角速度=0，即直线前进的情况，避免由于采样间隔的设置而跃过了这种特殊情况。

边迭代生成边比较，获得代价最小的路径，存放在best_traj。

执行完成后，也跳出了“未处于逃逸状态”这个判断结构。

    double impossible_cost = path_map_.obstacleCosts();

    //if we're performing an escape we won't allow moving forward
    if (!escaping_) {
      //【循环所有速度和角速度、打分】
      //外侧循环所有x速度
      for(int i = 0; i < vx_samples_; ++i) {
        //x速度循环内部遍历角速度
        //①角速度=0
        vtheta_samp = 0;
        //传入当前位姿、速度、加速度限制，采样起始x向速度、y向速度、角速度，代价赋给comp_traj
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);//【generateTrajectory函数如何实现-重点！】参数：位置、采样起始点、加速度

        //对比生成路径和当前最优路径的分数，如果生成的路径分数更小，就把当前路径和最优路径交换
        //这里会将第一次生成路径的代价赋给best_traj
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }

        //②角速度=下界
        vtheta_samp = min_vel_theta;
        //接下来迭代循环生成所有角速度的路径、打分
        for(int j = 0; j < vtheta_samples_ - 1; ++j){
          generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
              acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

          //同样地，如果新路径代价更小，和best_traj作交换
          if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
            swap = best_traj;
            best_traj = comp_traj;
            comp_traj = swap;
          }
          //遍历角速度
          vtheta_samp += dvtheta;
        }
        //遍历x速度
        vx_samp += dvx;
      }

非全向机器人不考虑y向线速度，会跳过这个判断结构。

      //只对holonomic robots迭代循环y速度，一般的机器人没有y速度
      if (holonomic_robot_) {
        //explore trajectories that move forward but also strafe slightly
        vx_samp = 0.1;
        vy_samp = 0.1;
        vtheta_samp = 0.0;
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }

        vx_samp = 0.1;
        vy_samp = -0.1;
        vtheta_samp = 0.0;
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
          swap = best_traj;
          best_traj = comp_traj;
          comp_traj = swap;
        }
      }
    } // end if not escaping

接下来，继续考虑线速度=0（原地旋转）的情况，来考虑一下这种情况什么时候会发生：

• 当TrajectoryPlannerROS中，位置已经到达目标（误差范围内），姿态已达，则直接发送0速；姿态未达，则调用降速函数和原地旋转函数，并调用checkTrajectory函数检查合法性，直到旋转至目标姿态。而checkTrajectory函数调用的是scoreTrajectory和generateTrajectory，不会调用createTrajectory函数，所以，快要到达目标附近时的原地旋转，根本不会进入到这个函数的这部分来处理。

• 并且，由于局部规划器的路径打分机制（后述）是：“与目标点的距离”和“与全局路径的偏离”这两项打分都只考虑路径终点的cell，而不是考虑路径上所有cell的综合效果，机器人运动到一个cell上，哪怕有任何一条能向目标再前进的无障碍路径，它的最终得分一定是要比原地旋转的路径得分来得高的。

所以，这里的原地自转，是行进过程中的、未达目标附近时的原地自转，并且，是机器人行进过程中遇到障碍、前方无路可走只好原地自转，或是连原地自转都不能满足，要由逃逸状态后退一段距离，再原地自转调整方向，准备接下来的行动。一种可能情况是机器人行进前方遇到了突然出现而不在地图上的障碍。

在处理这种情况时，由于机器人原地旋转时的角速度限制范围要比运动时的角速度限制范围更严格，底盘无法处理过小的原地转速，故要注意处理这层限制。同样调用generateTrajectory函数，生成路径。

    //接下来产生机器人在原地旋转的轨迹
    vtheta_samp = min_vel_theta;
    vx_samp = 0.0;
    vy_samp = 0.0;

    double heading_dist = DBL_MAX;
    //循环所有角速度
    for(int i = 0; i < vtheta_samples_; ++i) {
      //强制最小原地旋转速度
      double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
        : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
      //产生遍历角速度的路径
      generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
          acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

同样，在迭代过程中用更好的路径来更新best_traj，但处理原地旋转情况时，由于一直在原地cell，所以路径的代价一直不变，所以重点在第二个if，它先获取路径终点的坐标，实际上也就是原地的x、y坐标，再加上heading_lookahead_×自转后姿态的三角函数，这个坐标跟原地的坐标关系如下：

然后再用计算结果的坐标，借助goal_map_，取最小值对应的结果，用来更新best_traj。

对于这个筛选策略，我的理解是：这个计算得到的相当于机器人原地旋转后“面向”的cell，它和机器人的距离是heading_lookahead_，由于goal_map_上的障碍物cell值为obstacleCosts（大于正常cell），所以经过迭代，必然会筛选掉计算结果是障碍物的情形，也就是机器人旋转后不会面向障碍物；同时筛选后，也能得到和目标点距离最短的计算结果，保证机器人在旋转后向前行进，距离目标点的路程更短。

（TrajectoryPlanner的路径打分策略是以下三者的综合：①与目标点距离、②与全局规划的距离、③障碍物，在上述原地自转情况下，可能是突发障碍物，此时①没有意义，而这种筛选策略能够将②和③纳入考虑，也比较合理。）

      //如果新路径代价更小
      if(comp_traj->cost_ >= 0
          && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
          && (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
        //获取新路径的终点（原地）
        double x_r, y_r, th_r;
        comp_traj->getEndpoint(x_r, y_r, th_r);
        //坐标计算
        x_r += heading_lookahead_ * cos(th_r);
        y_r += heading_lookahead_ * sin(th_r);
        unsigned int cell_x, cell_y;

        //转换到地图坐标系，判断与目标点的距离
        if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
          double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
          //取距离最小的，放进best_traj
          if (ahead_gdist < heading_dist) {
            if (vtheta_samp < 0 && !stuck_left) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
            else if(vtheta_samp > 0 && !stuck_right) {
              swap = best_traj;
              best_traj = comp_traj;
              comp_traj = swap;
              heading_dist = ahead_gdist;
            }
          }
        }
      }
      //角速度遍历
      vtheta_samp += dvtheta;
    }

轨迹生成已完成，接下来的工作可以概括为，如果轨迹cost非负，即找到有效轨迹，则将其返回；若找不到有效轨迹，进入逃逸状态，后退、原地自转，若找不到下一步的有效路径则再后退、自转，直到后退的距离或转过的角度达到一定标准，才会退出逃逸状态，重新规划前向轨迹。其中再加上震荡抑制。

    //检查最优轨迹的分数是否大于0
    if (best_traj->cost_ >= 0) {
      //抑制震荡影响：当机器人在某方向移动时，对下一个周期的与其相反方向标记为无效
      //直到机器人从标记震荡的位置处离开一定距离，返回最佳轨迹。
      
      //若轨迹的采样速度≤0
      if ( ! (best_traj->xv_ > 0)) {
        //若轨迹的角速度<0
        if (best_traj->thetav_ < 0) {
          if (rotating_right) {
            stuck_right = true;
          }
          rotating_right = true;
        } else if (best_traj->thetav_ > 0) {
          if (rotating_left){
            stuck_left = true;
          }
          rotating_left = true;
        } else if(best_traj->yv_ > 0) {
          if (strafe_right) {
            stuck_right_strafe = true;
          }
          strafe_right = true;
        } else if(best_traj->yv_ < 0){
          if (strafe_left) {
            stuck_left_strafe = true;
          }
          strafe_left = true;
        }

        //set the position we must move a certain distance away from
        prev_x_ = x;
        prev_y_ = y;
      }

      double dist = hypot(x - prev_x_, y - prev_y_);
      if (dist > oscillation_reset_dist_) {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
      }

      dist = hypot(x - escape_x_, y - escape_y_);
      if(dist > escape_reset_dist_ ||
          fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){
        escaping_ = false;
      }

      return *best_traj;
    }

来看上面的代码，当轨迹cost非负，即轨迹有效，进入下面的过程，嵌套的几个if结构作用是为“震荡”和“逃逸”做记录，不影响轨迹的发布，只要轨迹有效，都会执行到return *best_traj，返回轨迹。

那么来看这几个if块，第一大部分是记录震荡：“当采样速度v≤0” ，由于线速度采样范围是正的，当出现非正采样速度时，只可能是上一次的traj无效，发布了后退命令，进入逃逸模式，然后重新循环，跳过前向轨迹规划，进入原地自转模式，导致采样速度=0

所以可以说是，当原地自转时：

• 若角速度<0，标记rotating_right为真
• 若角速度>0，标记rotating_left为真
• 并记录当前自转的位置

跳出这块代码，再跳过全向机器人的部分，发现一模一样的代码又出现了一遍，再执行一次会导致（不过我不太理解为什么这样设计，让一样的代码出现两次…）：

• 若角速度<0，标记stuck_right为真
• 若角速度>0，标记stuck_left为真
• 并记录当前自转的位置

直到机器人离开记录的位置一段距离后，以上标志位才会恢复false。

对照一下前面原地旋转的代码：可以发现，一旦机器人在某处负向角速度自转，stuck_right被置真，那么在它未离开当前位置一定距离前，是无法用正向角速度更新best_traj的，只能用负向角速度来更新它。

可见，震荡抑制能够避免机器人在一个小范围内左右来回乱转。

            if (vtheta_samp < 0 && !stuck_left) {
             swap = best_traj;
             best_traj = comp_traj;
             comp_traj = swap;
             heading_dist = ahead_gdist;
           }
           else if(vtheta_samp > 0 && !stuck_right) {
             swap = best_traj;
             best_traj = comp_traj;
             comp_traj = swap;
             heading_dist = ahead_gdist;

    //【暂不考虑全向机器人，跳过这段】
    if (holonomic_robot_) {
      //if we can't rotate in place or move forward... maybe we can move sideways and rotate
      vtheta_samp = min_vel_theta;
      vx_samp = 0.0;

      //loop through all y velocities
      for(unsigned int i = 0; i < y_vels_.size(); ++i){
        vtheta_samp = 0;
        vy_samp = y_vels_[i];
        //sample completely horizontal trajectories
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
            acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

        //if the new trajectory is better... let's take it
        if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
          double x_r, y_r, th_r;
          comp_traj->getEndpoint(x_r, y_r, th_r);
          x_r += heading_lookahead_ * cos(th_r);
          y_r += heading_lookahead_ * sin(th_r);
          unsigned int cell_x, cell_y;

          //make sure that we'll be looking at a legal cell
          if(costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
            double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
            if (ahead_gdist < heading_dist) {
              //if we haven't already tried strafing left since we've moved forward
              if (vy_samp > 0 && !stuck_left_strafe) {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
                heading_dist = ahead_gdist;
              }
              //if we haven't already tried rotating right since we've moved forward
              else if(vy_samp < 0 && !stuck_right_strafe) {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
                heading_dist = ahead_gdist;
              }
            }
          }
        }
      }
    }

    if (best_traj->cost_ >= 0) {
      if (!(best_traj->xv_ > 0)) {
        if (best_traj->thetav_ < 0) {
          if (rotating_right){
            stuck_right = true;
          }
          rotating_left = true;
        } else if(best_traj->thetav_ > 0) {
          if(rotating_left){
            stuck_left = true;
          }
          rotating_right = true;
        } else if(best_traj->yv_ > 0) {
          if(strafe_right){
            stuck_right_strafe = true;
          }
          strafe_left = true;
        } else if(best_traj->yv_ < 0) {
          if(strafe_left){
            stuck_left_strafe = true;
          }
          strafe_right = true;
        }

        //set the position we must move a certain distance away from 设置走出多远之后，上述避免震荡的标记不再起作用
        prev_x_ = x;
        prev_y_ = y;

      }

      double dist = hypot(x - prev_x_, y - prev_y_);
      if(dist > oscillation_reset_dist_) {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
      }

      dist = hypot(x - escape_x_, y - escape_y_);
      if(dist > escape_reset_dist_ || fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
        escaping_ = false;
      }

      return *best_traj;
    }

轨迹有效的部分结束，当轨迹cost为负即无效时，执行接下来的部分，设置一个负向速度，产生让机器人缓慢退后的轨迹。此处也还是判断一下震荡距离。

    vtheta_samp = 0.0;
    vx_samp = backup_vel_;//后退速度
    vy_samp = 0.0;
    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
        acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);

    //if the new trajectory is better... let's take it
    /*
       if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
       swap = best_traj;
       best_traj = comp_traj;
       comp_traj = swap;
       }
       */
       
    swap = best_traj;
    best_traj = comp_traj;
    comp_traj = swap;

    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_) {
      rotating_left = false;
      rotating_right = false;
      strafe_left = false;
      strafe_right = false;
      stuck_left = false;
      stuck_right = false;
      stuck_left_strafe = false;
      stuck_right_strafe = false;
    }

若后退速度生成的轨迹的终点有效（> -2.0），进入逃逸状态，循环后退、自转，并且记录下的逃逸位置和姿态，只有当离开逃逸位置一定距离或转过一定角度，才能退出逃逸状态，再次规划前向速度。逃逸判断和震荡判断一样，也已在上面多次执行，只要发布best_traj前就执行一次判断。

若终点无效，返回负cost，本次局部规划失败。

    //仅当存在有效终点时才进入逃逸模式
    if (!escaping_ && best_traj->cost_ > -2.0) {
      escape_x_ = x;
      escape_y_ = y;
      escape_theta_ = theta;
      escaping_ = true;
    }

    dist = hypot(x - escape_x_, y - escape_y_);

    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
      escaping_ = false;
    }

若后退轨迹遇障，还是继续后退，因为后退一点后立刻就会进入原地自转模式。

    //if the trajectory failed because the footprint hits something, we're still going to back up
    if(best_traj->cost_ == -1.0)
      best_traj->cost_ = 1.0;

    return *best_traj;

  }

这个函数内容比较多，画张图总结一下主体部分：

进入逃逸状态后，如下（白色圆点代表检测退出条件）：

<4> TrajectoryPlanner::generateTrajectorie

这个函数根据给定的速度和角速度采样生成单条路径和其代价。

  void TrajectoryPlanner::generateTrajectory(
      double x, double y, double theta,
      double vx, double vy, double vtheta,
      double vx_samp, double vy_samp, double vtheta_samp,
      double acc_x, double acc_y, double acc_theta,
      double impossible_cost,
      Trajectory& traj) {

    boost::mutex::scoped_lock l(configuration_mutex_); //局部范围锁 

    // 记录初始时刻的位姿、速度、角速度
    double x_i = x; 
    double y_i = y;
    double theta_i = theta;
    double vx_i, vy_i, vtheta_i;
    vx_i = vx;
    vy_i = vy;
    vtheta_i = vtheta;

    //非全向机器人即线速度
    double vmag = hypot(vx_samp, vy_samp);

计算仿真步数和每一步对应的时间，朝向打分与否对应的步数计算方法略有不同。

    int num_steps;
    if(!heading_scoring_) {
      //sim_granularity_：仿真点之间的距离间隔
      //步数 = max(速度模×总仿真时间/距离间隔，角速度/角速度间隔)，四舍五入
      num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
    } else {
      //步数 = 总仿真时间/距离间隔，四舍五入
      num_steps = int(sim_time_ / sim_granularity_ + 0.5);
    }

    //至少一步
    if(num_steps == 0) {
      num_steps = 1;
    }

    //每一步的时间
    double dt = sim_time_ / num_steps;
    double time = 0.0;

    //声明轨迹
    traj.resetPoints();
    traj.xv_ = vx_samp;//线速度
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp;//角速度
    traj.cost_ = -1.0;//默认代价-1.0

    //初始化轨迹的代价
    double path_dist = 0.0;//路径距离
    double goal_dist = 0.0;//目标距离
    double occ_cost = 0.0;//障碍物代价
    double heading_diff = 0.0;//航向角

接下来循环生成轨迹，并计算轨迹对应的代价值，先将当前点从global系转换到地图系，若无法转换到地图上，直接结束轨迹生成，并返回代价-1.0。

    for(int i = 0; i < num_steps; ++i){
      //存储点的地图上的坐标
      unsigned int cell_x, cell_y;

      //不希望路径跑出已知地图
      //从当前位置开始，依次循环全局路径上的点
      //把当前位置(x_i,y_i)转换到地图上，如果无法转换，说明该路径点不在地图上，将其代价设置为-1.0，并return
      if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
        traj.cost_ = -1.0;
        return;
      }

转换到地图后，开始考虑机器人的大小，把该点扩张到机器人在该点的足迹范围，并调用footprintCost函数，获得机器人在该点时它的足迹所对应的代价，若返回-1，说明足迹遇障，直接返回-1。这个函数在WorldModel类中声明，在它的派生类CostmapModel中定义，将机器人所在的点扩张到该点足迹，并结合局部规划器的costmap的值返回足迹对应的代价，具体内容下一篇记录。

将路径上点的最大cost记录在occ_cost中，这一步完成了碰撞检测的工作。

      //检查当前路径上的点的合法性，每一个路径点→机器人的足迹，检测足迹的多边形里是否碰到障碍物，若遇障则返回负值
      double footprint_cost = footprintCost(x_i, y_i, theta_i);

      //若返回一个负值，说明机器人在路径上遇障，直接return
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;
        //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
        //it can actually cause the robot to hit obstacles. There may be something to be done to fix, but I'll have to
        //come back to it when I have time. Right now, pulling it out as it'll just make the robot a bit more conservative,
        //but safe.
        /*
        double max_vel_x, max_vel_y, max_vel_th;
        //we want to compute the max allowable speeds to be able to stop
        //to be safe... we'll make sure we can stop some time before we actually hit
        getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);

        //check if we can stop in time
        if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
          ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
          //if we can stop... we'll just break out of the loop here.. no point in checking future points
          break;
        }
        else{
          traj.cost_ = -1.0;
          return;
        }
        */
      }

      //更新occ_cost：max(max(occ_cost，路径足迹代价)，当前位置的代价)
      //也就是把所有路径点的最大障碍物代价设置为路径的occ_cost
      occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));

接下来考虑与全局路径及目标点的距离：

• 如果simple_attractor_开启，采用比较简单的追踪策略，只考虑与目标点之间的直线距离^2
• 如果simple_attractor_未开启，则借助goal_map_和path_map_获取该点与目标点及全局规划路径之间的距离，若超过范围，说明没有明确的目标路径，则该路径无效，直接返回-2.0

经过迭代，goal_dist和path_dist储存的都是路径上最后一个点的对应代价，也就是用这种方法评价一条路径时，若路径有效，【全局路径及目标点的距离】只与路径末点有关。

如果开启heading_scoring_即为朝向打分，只有当从开始生成该条路径计时，到达特定时刻时，才会进行唯一一次打分，headingDiff函数的具体过程是：

• 从全局路径终点（目标点）开始，当前点与全局路径上的各点依次连线获得cost，cost为正（无障碍）则立即计算：当前点与迭代到的点间的连线方向与当前点的姿态之差，返回给heading_diff并停止继续连线；
• 若所有连线cost都为负，返回极大值。

为朝向打分的轮次将不更新goal_dist和path_dist。

      /*********************这里负责更新路径和目标距离**********************/
      //如果只是想blindly follow的话，如下（求与目标间的三角距离），否则见后
      if (simple_attractor_) {
        //目标距离
        goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
          (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
          (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
          (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
      } else {
        //不只是blindly follow，“更新路径距离和目标距离”
        bool update_path_and_goal_distances = true;
        //如果有heading scoring，我们要求出航向角，并且为路径某点的路径距离和目标距离打分
        //如果为朝向打分
        if (heading_scoring_) { 
          /**********这里求出本次时间是否是要给航向打分的时间**********/
          //heading_scoring_timestep_是给朝向打分时在时间上要看多远
          //也就是在路径上走过一个特定时刻后，才为朝向打分一次
          //如果heading_scoring_timestep_≤时间<heading_scoring_timestep_+一次的时间间隔
          if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt) {
            //求出航向角
            heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
          } else {
            //不更新路径和目标距离
            update_path_and_goal_distances = false;
          }
        }

        if (update_path_and_goal_distances) { 
          //更新路径距离与目标距离（只考虑终点cell）
          path_dist = path_map_(cell_x, cell_y).target_dist;
          goal_dist = goal_map_(cell_x, cell_y).target_dist;

          //如果目标距离或路径距离≥impossible_cost（地图尺寸），代价设置为-2.0
          if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
          //            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
          //                goal_dist, path_dist, impossible_cost);
            traj.cost_ = -2.0;
            return;
          }
        }
      }
      /*****************************************************************/

经过检测，若该点足迹不遇障，且该点的goal_dist与path_dist存在，加入轨迹。

并通过函数计算该点的速度，速度计算函数使当前速度在dt时间内以加速度acc_x向采样速度靠近，到达采样速度后将不再改变。所以实际上每条轨迹都是一个由当前速度趋向并稳定在采样速度的过程。

接下来通过航迹推演公式计算下一个路径点的坐标，并将几个打分的项按比例加和，得到当前路径的cost，进入下一次迭代，循环往复填充路径坐标，并更新路径cost。

需要注意的是，这里对速度的计算与我们发布给机器人的速度无关，这里的速度只是为了推算下一个点，获得路径，而我们真正发布给机器人的速度是采样速度。真实世界里机器人由当前速度–>采样速度的过程对应我们地图上本次仿真的轨迹。

      //这个点有效，加入轨迹
      traj.addPoint(x_i, y_i, theta_i);

      //计算速度、角速度
      //computeNewVelocity函数作用：不管速度大于还是小于vx_samp，都让其以加速度acc_x接近vx_samp后稳定在vx_samp
      vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
      vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
      vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);

      //通过计算出的速度计算下一个位置、姿态
      x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
      y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
      theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);

      //增加时间
      time += dt;
    } // end for i < numsteps

    //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
    //计算生成路径的代价
    double cost = -1.0; 
    if (!heading_scoring_) {
      //代价=路径距离+目标距离+障碍物代价（乘以各自的比例）
      cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
    } else {
      //代价=路径距离+目标距离+障碍物代价+航向角（如果有航偏则会增大代价）
      cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
    }
    //设置称当前路径的代价
    traj.cost_ = cost;
  }

<5> TrajectoryPlanner::checkTrajectorie 与 scoreTrajectorie

这两个函数并未进行实际工作，checkTrajectory调用scoreTrajectory，scoreTrajectory调用generateTrajectory，生成单条路径并返回代价。它们是在足够接近目标时，局部规划器产生降速和自转时生成的对应速度的路径。

  bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
    Trajectory t;

    double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);

    //if the trajectory is a legal one... the check passes
    if(cost >= 0) {
      return true;
    }
    ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);

    //otherwise the check fails
    return false;
  }

  double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
      double vtheta, double vx_samp, double vy_samp, double vtheta_samp) {
    Trajectory t;
    double impossible_cost = path_map_.obstacleCosts();
    generateTrajectory(x, y, theta,
                       vx, vy, vtheta,
                       vx_samp, vy_samp, vtheta_samp,
                       acc_lim_x_, acc_lim_y_, acc_lim_theta_,
                       impossible_cost, t);

    // return the cost.
    return double( t.cost_ );
  }

【Something】

这部分内容常看常新，每一次看都会产生新问题，推翻上一次的一些理解。这次将自己的思路记录了下来，一定有不准确、不完善之处，欢迎指正、共同讨论。

Neo 2020.3