GeRoNa学习笔记（一）

GeRoNa简介

GeRoNa (Generic Robot Navigation)是一个模块化的机器人导航框架，包含路径规划、路径跟随(带避障)、以及各个模块之间交互的管理。它的设计理念是可以针对新的任务和机器人方便的扩进行展。

以下是该导航框架的逻辑框图：主要由三个部分组成，路径控制、路径规划、路径跟随。建图、定位以及障碍物检测是不包含在内的，需要依赖其他程序提供地图服务(mapserver)和位姿(pose)信息(主要是依赖tf_tree)。三个部分之间通过ROS中的Action机制传输数据，其具体形式与优点见官方wiki说明 http://wiki.ros.org/actionlib 。

path_control包 

 "path_control"包较为简单，但它是其中一个非常重要的部分，有“承上启下“的作用。其中运行了两个node："highlevel_dummy" 与 "path_control_node"。首先我们在rviz中点击目标位姿以后，会产生一个 "/move_base_simple/goal" 的 topic，"highlevel_dummy"接收这个msg，随后会通过 "/navigate_to_goal" 服务(action形式)将goal发送给"path_control_node"，同时该程序还控制规划失败以后是否要重新规划或者直接放弃退出。"/navigate_to_goal"的msg中除包含目标信息外，还包含了一些配置信息，例如与路径跟随相关的目标速度，与路径规划相关的障碍物膨胀半径等。

path_msgs::NavigateToGoalGoal goal;
        goal.goal.pose = *pose;
        goal.failure_mode = failure_mode_;
        goal.follower_options.velocity = target_speed_;
        goal.goal.planning_channel.data = pnh_.param("planning_channel", std::string(""));
        goal.goal.planning_algorithm.data = pnh_.param("planning_algorithm", std::string(""));

        goal.planner_options.grow_obstacles = pnh_.param("grow_obstacles", true);
        goal.planner_options.obstacle_growth_radius = pnh_.param("obstacle_radius", 1.0);

        goal.planner_options.goal_dist_threshold = pnh_.param("planner/goal_dist_threshold", 0.05);
        goal.planner_options.goal_angle_threshold_degree = pnh_.param("planner/goal_angle_threshold", 22.5);

        goal.planner_options.reversed = pnh_.param("planner/reversed", false);

        goal.planner_options.allow_forward = pnh_.param("planner/allow_forward", true);
        goal.planner_options.allow_backward = pnh_.param("planner/allow_backward", false);

        goal.planner_options.ackermann_la = pnh_.param("planner/ackermann_la", 1.2);
        goal.planner_options.ackermann_steer_steps = pnh_.param("planner/ackermann_steer_steps", 2);
        goal.planner_options.ackermann_max_steer_angle_degree = pnh_.param("planner/ackermann_max_steer_angle", 60);
        goal.planner_options.ackermann_steer_delta_degree = pnh_.param("planner/ackermann_steer_delta", 15);

在"path_control_node"节点收到上层的导航请求后，会进入"PathController::processGoal()" 函数中(pathcontroller.cpp)，通过服务 "/plan_path" 向路径规划节点发送请求，包括目标信息和与配置信息，然后一直等待下层规划程序的回应，直到返回规划成功的路径或者超时。获取到规划的结果后(规划失败或者包含规划成功的全局路径)，通过 "/follow_path" 服务向路径跟随节点 "path_follower" 发送请求，请求中包含规划成功的全局路径和路径跟随相关的配置。随后会保持等待，直到路径跟随节点报告任务结束。如果路径跟随节点报告了任务失败，则会根据配置选择是直接放弃本次规划还是尝试再次规划，即再次执行一遍"PathController::processGoal()" 函数)。

path_planner包

在launch文件"path_planner.launch"中可以通过配置5个参数，选择不同的规划方法：

    <!--
    Arguments select the mapping / SLAM algorithm to use.
    Only one launch file can be included, even if all paramters are set to true.
    -->
    <arg name="use_planner_cost" default="false" />
    <arg name="use_planner_dynamic" default="false" />
    <arg name="use_planner_static" default="false" />
    <arg name="use_planner_goal" default="false" />

    <!-- default: must be checked last -->
    <arg name="use_planner_default" default="true" />

5个不同的规划方法也对应着5个launch文件，默认为启动"planner.launch"，除"planner_cost.launch"的另外3个launch方法感觉都还不完善。"planner.launch"与"planner_cost.launch"中均是启动"path_planner_node"节点，区别在使用的地图不同。"planner.launch"方法订阅普通的 "/map" topic，而"planner_cost.launch"使用map_server，订阅"/dynamic_map/cost" server。以下主要分析普通的"planner.launch"方法。

首先在"planner_node.cpp"中定义了Planner类，它是其他规划算法的基类，其中定义了一些规划算法通用的操作，例如订阅地图服务更新地图、执行规划前的准备工作、可视化路线等，最核心的是action_server "/plan_path"的回调函数Planner::execute(const path_msgs::PlanPathGoalConstPtr &goal)，它是规划程序的入口，接收"path_control_node"节点请求的规划任务。以下是Planner::execute()函数中主要的执行流程图：

其实核心的路径搜索算法都被封装在另一个依赖的包中: "cslibs_path_planning"，其中包含了各种各样的搜索算法(Dijkstra、A*及其变种)，均以模板类的形式实现，以达到代码最大重用化。所以在planInstance()函数是一个模板函数，在PathPlanner::plan()函数中根据不同的运动模型向planInstance()函数传入不同的参数：

    switch(algorithm) {
    case Algo::ACKERMANN:
        return planInstance(algorithm, algo_ackermann, goal, from_world, to_world, from_map, to_map);
    case Algo::SUMMIT:
        return planInstance(algorithm, algo_summit, goal, from_world, to_world, from_map, to_map);
    case Algo::PATSY:
        return planInstance(algorithm, algo_patsy, goal, from_world, to_world, from_map, to_map);
    case Algo::PATSY_FORWARD:
        return planInstance(algorithm, algo_patsy_forward, goal, from_world, to_world, from_map, to_map);
    case Algo::SUMMIT_FORWARD:
        return planInstance(algorithm, algo_summit_forward, goal, from_world, to_world, from_map, to_map);
    case Algo::OMNI:
        return planInstance(algorithm, algo_omni, goal, from_world, to_world, from_map, to_map);
    case Algo::GENERIC:
        updateGenericParameters(goal);
        return planInstance(algorithm, algo_generic, goal, from_world, to_world, from_map, to_map);