能动就行地理解RoboRTS-0 roborts_planning

Welcome!

这一系列的Blog作为对于RoboRTS框架的理解，主要服务于CUHKSZ相关赛事的研发。由于时间的紧迫性，我们不考虑彻底掌握框架的细节。第一年从能用就行开始。

主要参考：Tutorial

roborts_planning

导航模块由全局路径规划以及局部路径规划组成
局部路径规划采用的是teb，着实不好改。。。

全局路径规划

“全局路径规划（简称全局规划）是导航系统中运动规划的第一个步骤，在给定目标位置后，根据感知的全局代价地图搜索得到一条无碰撞的最短路径（即一系列离散的坐标点），然后作为输入传递给局部轨迹规划模块控制机器人的具体运动。
全局路径规划模块位于roborts_planner包中，依赖roborts_msgs包中的Action消息与roborts_common包中的抽象工厂模式和参数读取，模块文件目录如下所示”

文件结构如下

└── 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              #Astar全局规划算法实现
    │   └── ...
    ├── config
    │   └── global_planner_config.prototxt # 全局规划参数配置文件
    └── proto  
        ├── global_planner_config.pb.cc
        ├── global_planner_config.pb.h
        └── global_planner_config.proto    # 全局规划参数定义文件

由此可见，主要可以改动的部分是具体算法的实现（Aplannner文件夹）以及核心功能节点。
首先看一下A planner的代码a_star_planner.cpp：

#include "a_star_planner.h"

namespace roborts_global_planner{

    using roborts_common::ErrorCode;
    using roborts_common::ErrorInfo;

    AStarPlanner::AStarPlanner(CostmapPtr costmap_ptr) :
            GlobalPlannerBase::GlobalPlannerBase(costmap_ptr),
            gridmap_width_(costmap_ptr_->GetCostMap()->GetSizeXCell()),
            gridmap_height_(costmap_ptr_->GetCostMap()->GetSizeYCell()),
            cost_(costmap_ptr_->GetCostMap()->GetCharMap()) {

        AStarPlannerConfig a_star_planner_config;
        std::string full_path = ros::package::getPath("roborts_planning") + "/global_planner/a_star_planner/"\
      "config/a_star_planner_config.prototxt";

首先是命名空间的建立。（Q:为什么要独立创建一个命名空间？）以及costmap的读取。

if (!roborts_common::ReadProtoFromTextFile(full_path.c_str(),
                                           &a_star_planner_config)) {
    ROS_ERROR("Cannot load a star planner protobuf configuration file.");
}
//  AStarPlanner param config

以上：报错信息的设置。

heuristic_factor_ = a_star_planner_config.heuristic_factor();
        inaccessible_cost_ = a_star_planner_config.inaccessible_cost();
        goal_search_tolerance_ = a_star_planner_config.goal_search_tolerance()/costmap_ptr->GetCostMap()->GetResolution();

• inaccessible_cost(int, 默认值: 253)： 不可通过（即视为致命障碍物）的地图栅格代价值
• heuristic_factor(double, 默认值: 1.0)： 启发因子，见启发函数h(n) = heuristic_factor * manhattan_distance
• goal_search_tolerance(double, 默认值: 0.45)：寻找新目标点的容忍距离（单位米），如果发送的目标点不可通过，则在此允许距离周围内找一个可以通过的新目标点

    AStarPlanner::~AStarPlanner(){
        cost_ = nullptr;
    }

    ErrorInfo AStarPlanner::Plan(const geometry_msgs::PoseStamped &start,
                                 const geometry_msgs::PoseStamped &goal,
                                 std::vector<geometry_msgs::PoseStamped> &path) {

        unsigned int start_x, start_y, goal_x, goal_y, tmp_goal_x, tmp_goal_y;
        unsigned int valid_goal[2];
        unsigned  int shortest_dist = std::numeric_limits<unsigned int>::max();
        bool goal_valid = false;

        if (!costmap_ptr_->GetCostMap()->World2Map(start.pose.position.x,
                                                   start.pose.position.y,
                                                   start_x,
                                                   start_y)) {
            ROS_WARN("Failed to transform start pose from map frame to costmap frame");
            return ErrorInfo(ErrorCode::GP_POSE_TRANSFORM_ERROR,
                             "Start pose can't be transformed to costmap frame.");
        }

以上：对于起始点的位置设置，错误信息。

余下的部分就是A*的实现了。