RoboRTS源码解读（三） roborts_planning

[imuncle]

源码地址：https://github.com/RoboMaster/RoboRTS/tree/ros/roborts_planning

---

该节点包含两部分：全局路径规划和局部路径规划，全局路径规划使用A*算法，局部路径规划使用TEB算法，分别在global_planner和local_planner中。

全局路径规划节点代码结构详解

1.文件目录

global_planner
├── CMakeLists.txt
├── global_planner_algorithms.h #包含实现具体算法类的头文件
├── global_planner_base.h       #全局规划算法的抽象类
├── global_planner_test.cpp     #全局规划的测试节点
├── global_planner_node.cpp     #全局规划核心功能执行的节点和Main函数
├── global_planner_node.h       
├── a_star_planner              #A*全局规划算法实现
│   ├── config
│   │   └── a_star_planner_config.prototxt
│   ├── proto
│   │   ├── a_star_planner_config.pb.cc
│   │   ├── a_star_planner_config.pb.h
│   │   └── a_star_planner_config.proto
│   ├── a_star_planner.cpp      #A*算法具体实现
│   ├── a_star_planner.h
│   ├── CMakeLists.txt
├── config
│   └── global_planner_config.prototxt # 全局规划参数配置文件
└── proto  
    ├── global_planner_config.pb.cc
    ├── global_planner_config.pb.h
    └── global_planner_config.proto    # 全局规划参数定义文件

2.代码结构

全局规划整个节点很简单，只有一个Actionlib server及A*算法的实现。

Actionlib server的实现见global_planner_node.cpp，当中单独开了一个线程PlanThread用于路径规划，另外有一份测试代码实现了Actionlib client，在global_planner_test.cpp中，给定目标位置，让server计算路径。

启动全局路径规划的代码如下：

void GlobalPlannerNode::PlanThread() {
  ROS_INFO("Plan thread start!");
  geometry_msgs::PoseStamped current_start;
  geometry_msgs::PoseStamped current_goal;
  std::vector<geometry_msgs::PoseStamped> current_path;
  // 省略...
  while (ros::ok()) {
    // 省略...
    {
      std::unique_lock<roborts_costmap::Costmap2D::mutex_t> lock(*(costmap_ptr_->GetCostMap()->GetMutex()));
      bool error_set = false;
      //Get the robot current pose
      while (!costmap_ptr_->GetRobotPose(current_start)) {
        if (!error_set) {
          ROS_ERROR("Get Robot Pose Error.");
          SetErrorInfo(ErrorInfo(ErrorCode::GP_GET_POSE_ERROR, "Get Robot Pose Error."));
          error_set = true;
        }
        std::this_thread::sleep_for(std::chrono::microseconds(1));
      }

      //Get the robot current goal and transform to the global frame
      current_goal = GetGoal();

      if (current_goal.header.frame_id != costmap_ptr_->GetGlobalFrameID()) {
        current_goal = costmap_ptr_->Pose2GlobalFrame(current_goal);
        SetGoal(current_goal);
      }

      //Plan
      error_info = global_planner_ptr_->Plan(current_start, current_goal, current_path);

    }

    // 省略...
  }
  ROS_INFO("Plan thread terminated!");
}

其中global_planner_ptr_->Plan(current_start, current_goal, current_path)这句话调用了a_star_planner.cpp中的Plan函数，该函数又调用SearchPath函数进行路径规划。

SearchPath才是最终A*算法的实现，关于该算法的讲解可以参考Introduction to the A* Algorithm，讲的浅显易懂。

这里也贴上实现代码：

ErrorInfo AStarPlanner::SearchPath(const int &start_index,
                                   const int &goal_index,
                                   std::vector<geometry_msgs::PoseStamped> &path) {

  g_score_.clear(); // 移动代价
  f_score_.clear(); // 移动代价+曼哈顿距离
  parent_.clear();
  state_.clear();
  gridmap_width_ = costmap_ptr_->GetCostMap()->GetSizeXCell();
  gridmap_height_ = costmap_ptr_->GetCostMap()->GetSizeYCell();
  ROS_INFO("Search in a map %d", gridmap_width_*gridmap_height_);
  cost_ = costmap_ptr_->GetCostMap()->GetCharMap();
  g_score_.resize(gridmap_height_ * gridmap_width_, std::numeric_limits<int>::max());
  f_score_.resize(gridmap_height_ * gridmap_width_, std::numeric_limits<int>::max());
  parent_.resize(gridmap_height_ * gridmap_width_, std::numeric_limits<int>::max());
  state_.resize(gridmap_height_ * gridmap_width_, SearchState::NOT_HANDLED);

  std::priority_queue<int, std::vector<int>, Compare> openlist;
  g_score_.at(start_index) = 0;
  openlist.push(start_index); // 已经到达的位置

  std::vector<int> neighbors_index;
  int current_index, move_cost, h_score, count = 0;

  while (!openlist.empty()) {
    current_index = openlist.top();
    openlist.pop();
    state_.at(current_index) = SearchState::CLOSED;

    // 当搜索到终点时停止搜索
    if (current_index == goal_index) {
      ROS_INFO("Search takes %d cycle counts", count);
      break;
    }

    // 寻找近邻的九个栅格（这里和上面网址里的有所不同，网址里寻找的是上下左右四个栅格
    GetNineNeighbors(current_index, neighbors_index);

    for (auto neighbor_index : neighbors_index) {

      if (neighbor_index < 0 ||
          neighbor_index >= gridmap_height_ * gridmap_width_) {
        continue;
      }

      if (cost_[neighbor_index] >= inaccessible_cost_ ||
          state_.at(neighbor_index) == SearchState::CLOSED) {
        continue;
      }

      GetMoveCost(current_index, neighbor_index, move_cost);

      if (g_score_.at(neighbor_index) > g_score_.at(current_index) + move_cost + cost_[neighbor_index]) {
        // parent的移动代价+本次移动代价+栅格本身代价
        g_score_.at(neighbor_index) = g_score_.at(current_index) + move_cost + cost_[neighbor_index];
        parent_.at(neighbor_index) = current_index;

        if (state_.at(neighbor_index) == SearchState::NOT_HANDLED) {
          GetManhattanDistance(neighbor_index, goal_index, h_score);
          f_score_.at(neighbor_index) = g_score_.at(neighbor_index) + h_score;
          openlist.push(neighbor_index);
          state_.at(neighbor_index) = SearchState::OPEN;
        }
      }
    }
    count++;
  }

  if (current_index != goal_index) {
    ROS_WARN("Global planner can't search the valid path!");
    return ErrorInfo(ErrorCode::GP_PATH_SEARCH_ERROR, "Valid global path not found.");
  }

  unsigned int iter_index = current_index, iter_x, iter_y;

  // 从遍历的结果中将路经找出来
  // 从终点开始往回找，根据栅格的父子关系查找
  geometry_msgs::PoseStamped iter_pos;
  iter_pos.pose.orientation.w = 1;
  iter_pos.header.frame_id = "map";
  path.clear();
  costmap_ptr_->GetCostMap()->Index2Cells(iter_index, iter_x, iter_y);
  costmap_ptr_->GetCostMap()->Map2World(iter_x, iter_y, iter_pos.pose.position.x, iter_pos.pose.position.y);
  path.push_back(iter_pos);

  while (iter_index != start_index) {
    iter_index = parent_.at(iter_index);
    costmap_ptr_->GetCostMap()->Index2Cells(iter_index, iter_x, iter_y);
    costmap_ptr_->GetCostMap()->Map2World(iter_x, iter_y, iter_pos.pose.position.x, iter_pos.pose.position.y);
    path.push_back(iter_pos);
  }

  // 把路径变成起点到终点
  std::reverse(path.begin(),path.end());

  return ErrorInfo(ErrorCode::OK);

}

3.运行

正常比赛只需要运行

rosrun roborts_planning global_planner_node

测试可以在此基础上再运行

rosrun roborts_planning global_planner_test

另外全局规划节点还集成类RVIZ可视化，只需要打开RVIZ订阅相应的话题即可。

4.依赖

该节点依赖于roborts_common、robot_msgs和roborts_costmap，请在编译前确保有这三个库。

局部路径规划节点代码结构详解

1.代码结构

local_planner
├── CMakeLists.txt
├── package.xml
├── config
│   └── local_planner.prototxt          #局部路径规划算法配置文件
├── include
│   └── local_planner
│       ├── proto 
│       │   ├── local_planner.pb.cc
│       │   ├── local_planner.pb.h
│       │   └── local_planner.proto     # 局部路径规划参数生成文件
│       ├── data_base.h                 # 局部路径规划数据结构,用于g2o构建边以及顶点
│       ├── data_converter.h            # 数据转换,将数据转换为data_base数据结构
│       ├── distance_calculation.h      # 计算二维点、线、几何图形之间的距离
│       ├── line_iterator.h             # 连续线段在离散栅格中的坐标计算
│       ├── local_planner_algorithms.h  # 局部路径规划算法头文件（所有算法头文件都应在此文件中引入）
│       ├── local_planner_base.h        # 局部路径规划算法父类
│       ├── local_planner_node.h        # 局部路径规划入口节点
│       ├── local_visualization.h       # 可视化局部路径规划结果
│       ├── obstacle.h                  # 二维障碍物信息
│       ├── odom_info.h                 # 里程计信息
│       ├── optimal_base.h              # 优化相关算法父类
│       ├── robot_footprint_model.h     # 机器人外形描述
│       ├── robot_position_cost.h       # 机器人所在位置代价计算，用于判断路径是否可行
│       └── utility_tool.h              # 通用函数
├── src
│   ├── local_planner_node.cpp          # ros node文件，负责局部路径规划内的逻辑调度
│   ├── vel_converter.cpp               # ros node文件, 仿真时将局部路径规划速度转换为“cmd_vel”发布
│   ├── teb_test.cpp                    # 算法timed elastic band 测试文件
│   └── ...                             # 其他与头文件一一对应的cpp文件
└── timed_elastic_band
    ├── CMakeLists.txt
    ├── config
    │   └── timed_elastic_band.prototxt # timed elastic band 配置文件
    ├── include
    │   └── timed_elastic_band
    │       ├── proto
    │       │   ├── timed_elastic_band.pb.cc
    │       │   ├── timed_elastic_band.pb.h
    │       │   └── timed_elastic_band.proto
    │       ├── teb_local_planner.h     # 算法timed elastic band实现，继承local_planner_base.h
    │       ├── teb_optimal.h           # g2o优化逻辑，继承optimal_base.h
    │       └── ...                     # 其他g2o的边以及顶点相关文件。
    └── src
        ├── teb_local_planner.cpp       # 算法timed elastic band实现
        ├── teb_optimal.cpp             # g2o优化逻辑
        └── teb_vertex_console.cpp      # 存储teb算法中机器人位姿和时间间隔

2.代码结构

局部路径规划的代码的结构较为繁多，代码分为两部分，一部分是ROS节点的实现，建立了一个Actionlib server用于响应路径规划请求，建立一个线程用于局部路径规划。Actionlib server很简单，这里不做过多描述，具体参考local_planner_node.cpp。

在路径规划线程中通过下面这句话执行规划：

roborts_common::ErrorInfo error_info = local_planner_->ComputeVelocityCommands(cmd_vel_);

这条语句调用了teb_local_planner.cpp，该文件的主要功能是更新机器人位置和路径规划的起点和终点，它初始化了一个g2o优化器optimal_，并在ComputeVelocityCommands函数中调用了它：

bool success = optimal_->Optimal(transformed_plan_, &robot_current_vel_, free_goal_vel_, micro_control);

这条语句调用了teb_optimal.cpp，这是teb算法的真正实现部分，该文件结合了timed_elastic_band/include下的头文件和teb_vertex.console.cpp文件，利用g2o框架实现teb算法。

在阅读下面内容前，请先学习teb算法和g2o的基本操作。

teb算法可以参考论文：C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153.

g2o可以参考论文：《g2o : A General Framework for Graph Optimization》

另外推荐两个博客：一个是讲teb的：Timed-Elastic-Band局部路径规划算法，一个是讲g2o的：graph slam tutorial : 从推导到应用1。

需要注意的是，代码中只实现了最简单的teb算法，并没有加入后面的拓扑结构分析实现绕过障碍物。

3.TEB算法的实现

这部分的一大特点是函数重载较多，因为有好几种不同的代价函数，也就是g2o超图的边。

整个算法的入口是bool TebOptimal::Optimal函数，从teb_optimal.h中可以看到，这个函数有两个重载，不过都大同小异，先调用InitTEBtoGoal初始化路径上的机器人位姿和两位姿之间的时间间隔，然后初始化起点速度和终点速度，最后调用OptimizeTeb函数。

OptimizeTeb函数是真正的teb核心算法，其实现如下：

bool TebOptimal::OptimizeTeb(int iterations_innerloop,
                             int iterations_outerloop,
                             bool compute_cost_afterwards,
                             double obst_cost_scale,
                             double viapoint_cost_scale,
                             bool alternative_time_cost) {
  if (optimization_info_.optimization_activate() == false){
    return false;
  }

  bool success = false;
  optimized_ = false;

  double weight_multiplier = 1.0;

  for (int i=0; i<iterations_outerloop; ++i) {
    if (trajectory_info_.teb_autosize()) {
      vertex_console_.AutoResize(trajectory_info_.dt_ref(), trajectory_info_.dt_hysteresis(),
                                 trajectory_info_.min_samples(), trajectory_info_.max_samples());
    }
    // 建图
    success = BuildGraph(weight_multiplier);
    if (!success) {
      ClearGraph();
      return false;
    }
    // 优化（路径解算）
    success = OptimizeGraph(iterations_innerloop, false);
    if (!success) {
      ClearGraph();
      return false;
    }
    optimized_ = true;

    // 清除超图
    ClearGraph();

    weight_multiplier *= optimization_info_.weight_adapt_factor();
  }

  return true;
}

上面的代码中最关键的就是建图和优化两个步骤，建图函数如下：

bool TebOptimal::BuildGraph(double weight_multiplier) {
  if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) {
    ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!");
    return false;
  }

  AddTebVertices();
  if (obstacles_info_.legacy_obstacle_association()) {
    AddObstacleLegacyEdges(weight_multiplier);
  } else {
    AddObstacleEdges(weight_multiplier);
  }

  AddVelocityEdges();

  AddAccelerationEdges();

  AddTimeOptimalEdges();

  AddKinematicsDiffDriveEdges();

  AddPreferRotDirEdges();

  return true;
}

这一步就是将路径的一系列约束加进去，比如与障碍物保持一定距离，速度、加速度限制、时间最小约束、最小转弯半径约束、旋转方向约束等，这些都转换成了代价函数放在g2o中，g2o优化时会使这些代价函数达到最小。

具体的每一种约束的结构和实现见local_planner/timed_elastic_band/include/timed_elastic_band/下的头文件，可结合论文公式理解。

优化函数如下：

bool TebOptimal::OptimizeGraph(int no_iterations, bool clear_after) {
  if (robot_info_.max_vel_x() < 0.01) {
    ROS_WARN("Robot Max Velocity is smaller than 0.01m/s");
    if (clear_after) {
      ClearGraph();
    }
    return false;
  }

  if (!vertex_console_.IsInit() || vertex_console_.SizePoses() < trajectory_info_.min_samples()) {
    ROS_WARN("teb size is too small");
    if (clear_after) {
      ClearGraph();
    }
    return false;
  }

  optimizer_->setVerbose(optimization_info_.optimization_verbose());
  optimizer_->initializeOptimization();

  int iter = optimizer_->optimize(no_iterations);
  if(!iter) {
    ROS_ERROR("optimize failed");
    return false;
  }
  if (clear_after) {
    ClearGraph();
  }

  return true;
}

完成路径优化后，就根据局部路径中的前两个位姿计算当前机器人的速度和加速度，使用GetVelocity函数。

4.运行

正常比赛时运行

rosrun roborts_planning local_planner_node

可以运行

rosrun roborts_planning teb_test

然后打开rviz：

rviz -d teb_test.rviz

直观感受teb算法的强大。

5.依赖

该节点依赖于roborts_common、robot_msgs和roborts_costmap，请在编译前确保有这三个库。