DWA和base_local_planner的關(guān)系

在base_local_planner包中有兩個文件叫trajectory_planner.cpp 以及對應的ros實現(xiàn)，其和DWA是同一層的。
由于nav_core提供了統(tǒng)一的接口，因此我們可以先看看統(tǒng)一的接口有哪些，那我們便知道每一個算法里比較重要的函數(shù)有哪些。

nav_core包里的base_local_planner.h文件

//最為關(guān)鍵的地方，計算機器人下一刻的速度
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
//判斷是否到達目標點
virtual bool isGoalReached() = 0;
//加載全局路徑
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
//初始化
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;

下面我們就先看看base_local_planner的computeVelocityCommands的主要實現(xiàn)框架

bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
    //檢查初始化、檢查是否已經(jīng)到達目標點...略
    transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan);
    //如果已經(jīng)到達目標點，姿態(tài)還沒到
    if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) 
    {
        tc_->updatePlan(transformed_plan);
        //所以這個函數(shù)里最關(guān)鍵的子函數(shù)是findBestPath
        Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
        return true;
    }
    tc_->updatePlan(transformed_plan);
    Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
    //然后又是轉(zhuǎn)換，然后就發(fā)布出速度了...
}

接下來我們看一下TrajectoryPlanner的findBestPath的實現(xiàn)框架，Come on~

Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities)
{
    //...
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2],
        acc_lim_x_, acc_lim_y_, acc_lim_theta_);
    
    //...
}

順藤摸瓜，一睹createTrajectories的內(nèi)部實現(xiàn)，這個函數(shù)是軌跡采樣算法，可以說是一個非常關(guān)鍵的函數(shù)。

Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
      double vx, double vy, double vtheta,
      double acc_x, double acc_y, double acc_theta) 
{
    //檢查最終點是否是有效的，判斷變量在updatePlan中被賦值
    if( final_goal_position_valid_ )
    {
      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算法， sim_peroid_是1/controller_frequency_，暫時不清楚sim_period_和sim_time_的區(qū)別
    if (dwa_)
    {
      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
    {
      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_);
    }
    //...先忽略其中的邏輯，只要知道按照不同的規(guī)則生成路徑，調(diào)用的子函數(shù)是generateTrajectory
}

這個子函數(shù)的作用就是生成路徑，并且評分

void TrajectoryPlanner::generateTrajectory
{
    //主要有兩大作用：
    //生成路徑和速度
    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);
    //對路徑進行評分
    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_;
    }
    //這里的順序與源碼不同，我覺得總分來看更有組織性
    
    //該軌跡與全局路徑的相對距離
    path_dist = path_map_(cell_x, cell_y).target_dist;
    //距離目標點距離
    goal_dist = goal_map_(cell_x, cell_y).target_dist;
    //離障礙物距離
    double footprint_cost = footprintCost(x_i, y_i, theta_i);
    occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
}

綜上所述，其整一個邏輯順序就是computeVelocityCommands->findBestTrajectory --> createTrajectories --> generateTrajectory

最終，選擇分數(shù)最低的軌跡，發(fā)布出去。這便是整個局部規(guī)劃器的實現(xiàn)思路和邏輯。下一篇，談談DWA。