base_local_planner源码解读

ROS 专栏收录该内容
8 篇文章 0 订阅

现在网上看到的对于base_local_planner的解读很多都是概念性的,或者直接翻译自英文wiki。每当打开源码的时候看到这么多类和方法真的让人头大。

现在因为工作需要对base_local_planner做自己的修改,正好也没有人做过完整的源码解读的工作,我就决定试试看。这个包比之前的robot_post_ekf大太多,不太可能每个参数声明的时候都去做解释,所以只会在程序涉及到的时候才会说明。对应的文件会用文件表示,对应的函数用粗体表示,对应的变量用斜体表示。

因为本人水平有限,在解读的过程中难免会有疏漏,欢迎在评论区交流或者直接邮箱联系我:zhxue146@foxmail.com。



绪论

首先明确,navigation中的全局路径规划、局部路径规划以及恢复行为均为插件形式,这里有我写的ROS内导航插件机制的短文,最好先看一看。

navigation中的相关package

主要涉及其中三个包,其中含义如下(转载博客链接):

  1. nav_core:

该包定义了整个导航系统关键包的接口函数,包括base_global_planner, base_local_planner以及recovery_behavior的接口。里面的函数全是虚函数,所以该包只是起到规范接口的作用,真正功能的实现在相应的包当中。之后添加自己编写的全局或局部导航算法也需要在这里声明

  1. move_base

这个是整个navigation stack当中进行宏观调控的看得见的手。它主要干的事情是这样的:
 维护一张全局地图(基本上是不会更新的,一般是静态costmap类型),维护一张局部地图(实时更新,costmap类型),维护一个全局路径规划器global_planner完成全局路径规划的任务, 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
 然后提供一个对外的服务,负责监听nav_msgs::goal类型的消息,然后调动全局规划器规划全局路径,再将全局路径送进局部规划器,局部规划器结合周围障碍信息(从其维护的costmap中查询),全局路径信息,目标点信息采样速度并评分获得最高得分的轨迹(即是采样的最佳速度),然后返回速度值,由move_base发送Twist类型的cmd_vel消息上,从而控制机器人移动,完成导航任务。

  1. base_local_planner

完成局部窗口内的路径规划任务,机器人行动速度的具体生成在此包当中完成。目前有两种局部路径规划算法实现,一是航迹推算法(TrajectoryROS),一是动态窗口法(DWA),该包内部的默认实现是航迹推算法,但是留出了DWA的定义接口,DWA的实现在dwa_local_planner中。

用简单的话来说,move_base中会对local_planner进行声明并且调用nav_core中定义的接口函数,至于怎么具体实现,本文的情况中使用就是自带的base_local_planner。

函数调用顺序:从move_base开始

既然对于base_local_planner的声明与调用均在move_base中,那么首先进入mova_base包中去一探究竟。move_base采用actionlib机制,最好先去了解一下。

move_base_node.cpp是对于move_base.cpp的ros包装,所以直接进入move_base.cpp。关于整个move_base内部运行的顺序,请看这里,这里摘录如下:

从构造函数的actionServer 和线程函数planThread(等待executeCb通知他工作,规划一下路径,这个是global plan)

 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base",   boost::bind(&MoveBase::executeCb, this, _1), false);
 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

每次有goal发布的时候,都会去调用executeCb,executeCb会去调用executeCycle
进行local plan,发布cmd_vel的topic,根据小车处于的状态,进行相应的实现(会进行plan,control,clear obstacle)

因为本文讨论的是local_planner,会把精力集中在move_base对local_planner的调用上。在move_base.cpp的构造函数中129-132行声明local_planner、初始化并设定全局cost_map:

      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
      tc_->setGlobalCostmap(planner_costmap_ros_);

reconfigureCB中的tc_是用来动态修改参数的,相当于重新声明local_planner,与构造函数中形式一样。因此重点在executeCycle上,其中tc_分别出现了三次,如下:

    //第821行,较为简单,单纯的清除上一次全局规划并且设定为新的全局规划
    if(!tc_->setPlan(*controller_plan_)){
      
    //第856行,简单,返回是否到达终点的flag
    if(tc_->isGoalReached()){
      
    //第882行,计算计划和速度的重点
    if(tc_->computeVelocityCommands(cmd_vel)){

现在,整个move_base中对local_planner的调用顺序已经清晰,让我们回到正题,看看base_local_planner是怎么实现的。


构造函数与初始化

首先先看看英文wiki中如何使用base_local_planner:

#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/trajectory_planner_ros.h>

...

tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS costmap("my_costmap", tf);

base_local_planner::TrajectoryPlannerROS tp;

首先生成在trajectory_planner_ros.cpp中的base_local_planner::TrajectoryPlannerROS实例tp,base_local_planner::TrajectoryPlannerROS是对base_local_planner::TrajectoryPlanner的ROS包装。

在生成实例的过程中声明一些变量,最重要的是参数如下:

      WorldModel* world_model_; //控制器将会使用的世界模型
      TrajectoryPlanner* tc_; //轨迹控制器本体,同时也声明了许多许多个将要用的参数,简直没法展开说,要用的时候再说吧

轨迹控制器本体在trajectory_planner.cpp中,这里真正的涉及到了轨迹的生成与计算,以后我们会再见到它。

tp.initialize("my_trajectory_planner", &tf, &costmap);

我们可以对比move_base.cpp129-132行:

      tc_ = blp_loader_.createInstance(local_planner);
      ROS_INFO("Created local_planner %s", local_planner.c_str());
      tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
      //多了这一句,在nav_core中定义这个虚函数,然而base_local_planner并没有重载,所以这里并没有意义。
      tc_->setGlobalCostmap(planner_costmap_ros_);

initialize开始真正的base_local_planner,即trajectory_planner_ros.cpp第84-245行。首先将会是很多很多的参数读取以及纠正不科学的参数。在读取这些参数以后,我们终于开始正式生成轨迹和评价。

在第230行正式对base_local_planner进行声明:

      tc_ = new TrajectoryPlanner(*world_model_, *costmap_, footprint_spec_,
          acc_lim_x_, acc_lim_y_, acc_lim_theta_, sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,
          gdist_scale, occdist_scale, heading_lookahead, oscillation_reset_dist, escape_reset_dist, escape_reset_theta, holonomic_robot,
          max_vel_x, min_vel_x, max_vel_th_, min_vel_th_, min_in_place_vel_th_, backup_vel,
          dwa, heading_scoring, heading_scoring_timestep, meter_scoring, simple_attractor, y_vels, stop_time_buffer, sim_period_, angular_sim_granularity);

其构造函数在trajectory_planner_ros.cpp第141-172行,将许多flag定义为false,之后是187行:

    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);

计算了costmap中需要使用的内接圆与外切圆半径。至此,构造函数与初始化结束。


速度与路径计算:setPlan

setPlan在trajectory_planner_ros.cpp中的第372-第525行,在这里将会计算机器人实际运行的速度,发布局部路径规划,可以说是base_local_planner的核心,接下来我会放慢点节奏,好好咀嚼这里。

首先将机器人的姿态以及全局路径规划转换到global_frame_上。

    if (!costmap_ros_->getRobotPose(global_pose)) {
      return false;
    }

    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    //get the global plan in our frame
    if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {
      ROS_WARN("Could not transform the global plan to the frame of the controller");
      return false;
    }

第395-419行,得到机器人当前速度以及终点的(x,y, θ \theta θ)。第422-473行,查看机器人是否到达终点,如果到达了,就让机器人停下来。第475-478行:

    //将全局路径拷贝进来,并认为全军路径的最后一个点就是终点。
    tc_->updatePlan(transformed_plan);

    //计算应该跟随什么轨迹,给定当前机器人位置和朝向,计算机器人应该跟随的好轨迹。
    Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);

findBestPathtrajectory_planner.cpp中第906至第977行,终于我们开始正式计算轨迹以及速度分量,将函数摘抄如下:

  //计算应该跟随什么轨迹,给定当前机器人位置和朝向,计算机器人应该跟随的好轨迹。
  Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
      tf::Stamped<tf::Pose>& drive_velocities){
	
	//将当前机器人位置和方向转变成float形式的向量
    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标记
    for (unsigned int i = 0; i < footprint_list.size(); ++i) {
      path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
    }

    //确保我们根据全局计划更新map并且计算代价
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");

    //找到代价最低的轨迹。输入分别是目前机器人位置,速度以及加速度限制,生成诸多可能轨迹,选取其中打分最高的。这里也就是最关键的一步。
    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);
    }
    */
	// 如果最后打分如果小于0,说明所有的路径都不可用
    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;
  }

轨迹返回:createTrajectories

createTrajectories是整个dwa_local_controller以及base_local_controller的关键,它在trajectory_planner_ros第535-第902行。

首先第539-第560行,读取数据,设定这一次仿真中可以到达的最大线速度和角速度。

第563-第569行,首先根据vx速度以及 θ \theta θ的采样数,分别计算出每一次线速度和角速度的变化值dvx和dvtheta。并把最小的vx、vy、v θ \theta θ作为初始采样。

第584-第645行,如果机器人没有在逃离,首先假设机器人会向前,那么按照按照顺序对所有采样到的vx,对每一个vx都去尝试v θ \theta θ使用generateTrajectory生成轨迹并打分,保留分数最高的轨迹。如果是柔性机器人那么还有vy方向的采样和打分。第655-第697行,机器人不向前只旋转的情况下,对这些轨迹进行打分。保留下分数最高的轨迹。

第700-第846行,检查最优轨迹的分数是否大于0,也就是正常。如果正常的话,那么为了抑制震荡影响,当机器人在某方向移动时,对下一个周期的与其相反方向标记为无效,直到机器人从标记震荡的位置处离开一定距离,(声明与定义在trajectory_planner.h中第271-第275行),返回最佳轨迹。

最后,当最优轨迹不可用时,就让机器人缓慢退后。

轨迹生成与打分:generateTrajectory

现在来看看怎么具体生成轨迹并打分的,该函数定义在trajectory_planner.h中第247-249行,连带说明如下:

      //double vx_samp, double vy_samp, double vtheta_samp
      //分别为这次采样中使用的vx, yv, vtheta,生成的轨迹以引用的形式传回。
      void 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);

这一部分可以说是base_local_planner的核心,所以我全部拷贝过来,慢慢做解释:

  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_);
	
	//记录初始时刻的x、y、theta、vx、vy、vtheta
    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_) {
      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;

    //初始化这个轨迹的cost
    double path_dist = 0.0;
    double goal_dist = 0.0;
    double occ_cost = 0.0;
    double heading_diff = 0.0;

    for(int i = 0; i < num_steps; ++i){
    
      unsigned int cell_x, cell_y;

      //不希望路径跑出已知地图
      if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
        traj.cost_ = -1.0;
        return;
      }

      //检查当前点路径的合法性
      double footprint_cost = footprintCost(x_i, y_i, theta_i);

      //如果遇到了障碍物,那么会返回一个负数
      if(footprint_cost < 0){
        traj.cost_ = -1.0;
        return;

	  //更新occ_cost
      occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));

      //如果只是想简简单单的跟随终点的话,这里调整为true即可
      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 {

        bool update_path_and_goal_distances = true;

        // 是否为朝向打分?
        if (heading_scoring_) {
          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) {
          //更新路径与目标的距离
          path_dist = path_map_(cell_x, cell_y).target_dist;
          goal_dist = goal_map_(cell_x, cell_y).target_dist;

          //如果一个路径上的点没法明确到达在终点,他就是无效的
          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;
          }
        }
      }

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

      //计算速度
      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;
    } 
    
    double cost = -1.0;

	//更新cost
    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;
  }

总结与梳理

至此,整个计算过程结束。接下来会用流程图明确到底如何计算出速度的:
在这里插入图片描述

  • 9
    点赞
  • 6
    评论
  • 46
    收藏
  • 打赏
    打赏
  • 扫一扫,分享海报

©️2022 CSDN 皮肤主题:深蓝海洋 设计师:CSDN官方博客 返回首页

打赏作者

zhxue_11

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值