Gerona中path_follower模块源码解析

Gerona的path_follower模块源码解析

​ GERONA中path_follower包的代码结构如下：

​ path_follower模块又分为几个子模块：collision_avoidance、controller、factory、local_planner、supervisor，每一个子模块实际都是定义好的一个类。所有程序的入口（main函数）在follower_node.cpp文件中；path_follower_server.cpp中定义了PathFollowerServer类，实现了一个Action server使用actionlib库与path_control节点通信（msgs名字为"follow_path"），所以该类中定义了action的回调函数以及一个*PathFollowerServer::spin()函数，其中执行所有的follow任务；pathfollower.cpp中定义了PathFollower类，所有的子模块在这里被实例化（构建一个对象）并被使用，PathFollower类与PathFollowerServer类一起在main()*函数中被实例化，它的构造函数被调用，*PathFollower::update()*函数是程序的入口，在“boost::variant<FollowPathFeedback, FollowPathResult> PathFollowerServer::update()”函数中以50Hz的频率执行，返回FollowPath的状态或结果。

1. pathfollower.cpp分析

​ pathfollower.cpp文件中都是PathFollower类成员函数的定义，首先看它的构造函数：

PathFollower::PathFollower(ros::NodeHandle &nh):
    node_handle_(nh),
    // 这里创建一个pose_tracker，在FollowerFactory::construct()函数中被collision_avoider、local_planner、controller用于初始化
    pose_tracker_(new PoseTracker(*PathFollowerParameters::getInstance(), nh)),
    follower_factory_(new FollowerFactory(*this)),
    supervisors_(new SupervisorChain()),
    course_predictor_(new CoursePredictor(pose_tracker_.get())),
    visualizer_(Visualizer::getInstance()),
    opt_(*PathFollowerParameters::getInstance()),  // 此处获取launch文件中的param，构造PathFollowerParameters类的一个实例
    opt_l_(*LocalPlannerParameters::getInstance()),
    path_(new Path(opt_.world_frame())),
    pending_error_(-1),
    is_running_(false),
    vel_(0.0)
{
}

​ 在构造函数的初始化列表中就已经构造各子模块的实例，pose_tracker用于查询机器人当前的全局位姿，即map_frame到robot_frame的tf变换；follower_factory用于实例化collision_avoidance、controller和local_planner，会根据传入的参数创建不同类型的对象，例如controller根据不同的车辆模型（ackerman、omnidriver等）有不同的实现；supervisors是一个独立的监视工具，它可以监视机器人的运行情况，并且在异常情况下（脱离跟踪路径太远、与障碍物发送碰撞等）发出警报终止任务；visualizer是一个可视化工具，其中定义了许多图形绘制的方法，画线、画弧、画箭头甚至写画文字，发布”visualization_msgs::Marker“类型的topic，消息为"visualization_marker"和"visualization_marker_array"，在rviz中订阅；opt用于从参数服务器中（launch文件中定义）获取和follower相关的参数；opt_l用于获取和local_planner相关的参数；**path_**用于存储上一层传递过来的全局路径，在PathFollowerServer类中定义的action回调函数*PathFollowerServer::followPathGoalCB()*中被赋值。

​ *PathFollower::update()*函数是所有算法执行的地方，期望运行频率50Hz，但实际受限于算法运行速度，达不到50Hz。

boost::variant<FollowPathFeedback, FollowPathResult> PathFollower::update()
{
    ROS_ASSERT(current_config_);

    FollowPathFeedback feedback;
    FollowPathResult result;

    if(!is_running_) {
        start();
    }

    if (path_->empty()) {
        ROS_ERROR("tried to follow an empty path!");
        stop(FollowPathResult::RESULT_STATUS_INTERNAL_ERROR);
        return result;
    }

    //! 查找tf_tree获取机器人的pose(查找tf-tree world_frame->robot_frame)
    if (!pose_tracker_->updateRobotPose()) {
        ROS_ERROR("do not known own pose");
        stop(FollowPathResult::RESULT_STATUS_SLAM_FAIL);

    } else {
        course_predictor_->update();
        current_config_->controller_->setCurrentPose(pose_tracker_->getRobotPose());
    }

    // Ask supervisor whether path following can continue
    Supervisor::State state(pose_tracker_->getRobotPose(),
                            path_,
                            obstacle_cloud_,
                            feedback);

    Supervisor::Result s_res = supervisors_->supervise(state);
    if(!s_res.can_continue) {
        ROS_WARN("My supervisor told me to stop.");
        stop(s_res.status);

        return result;
    }

    if(current_config_->local_planner_->isNull()) {
        // 不使用局部路径规划
        //! 接收上层规划的全局路径后，输出运动控制指令
        is_running_ = execute(feedback, result);

    } else  {
        //End Constraints and Scorers Construction
        publishPathMarker();  // 发布topic "visualization_marker",ns为 "global robot path"

        if(obstacle_cloud_ != nullptr){
            current_config_->local_planner_->setObstacleCloud(obstacle_cloud_);
        }
        if(elevation_map_ != nullptr){
            current_config_->local_planner_->setElevationMap(elevation_map_);
        }

        // 获取LocalPlanner的参数
        const LocalPlannerParameters& opt_l = *LocalPlannerParameters::getInstance();

        if(opt_l.use_velocity()){
            //current_config_->local_planner_->setVelocity(pose_tracker_->getVelocity().linear);
            current_config_->local_planner_->setVelocity(pose_tracker_->getVelocity());
        }

        bool path_search_failure = false;
        try {
            // 进行局部规划
            Path::Ptr local_path = current_config_->local_planner_->updateLocalPath();
//            ROS_INFO("updateLocalPath...");

            path_search_failure = local_path && local_path->empty();
            if(local_path && !path_search_failure) {
                // 若局部路径规划成功
                path_msgs::PathSequence path;
                path.header.stamp = ros::Time::now();
                path.header.frame_id = getFixedFrameId();
                for(int i = 0, sub = local_path->subPathCount(); i < sub; ++i) {
                    const SubPath& p = local_path->getSubPath(i);
                    path_msgs::DirectionalPath sub_path;
                    sub_path.forward = p.forward;
                    sub_path.header = path.header;
                    for(const Waypoint& wp : p.wps) {
                        geometry_msgs::PoseStamped pose;
                        pose.pose.position.x = wp.x;
                        pose.pose.position.y = wp.y;
                        pose.pose.orientation = tf::createQuaternionMsgFromYaw(wp.orientation);
                        sub_path.poses.push_back(pose);
                    }
                    path.paths.push_back(sub_path);
                }
                local_path_pub_.publish(path);  // 发布局部路径，topic为"local_path"
            }

            //! 接收上层规划的路径(全局或局部)以后，输出运动控制指令
            is_running_ = execute(feedback, result);
//            ROS_INFO("PathFollower::execute...");

        } catch(const std::runtime_error& e) {
            ROS_ERROR_STREAM("Cannot find local_path: " << e.what());
            path_search_failure = true;
        }

        if(path_search_failure) {
            // 若搜索失败
            ROS_ERROR_STREAM_THROTTLE(1, "no local path found.");
            feedback.status = path_msgs::FollowPathFeedback::MOTION_STATUS_NO_LOCAL_PATH;
            current_config_->controller_->stopMotion();

            // publish an empty path
            path_msgs::PathSequence path;
            path.header.stamp = ros::Time::now();
            path.header.frame_id = getFixedFrameId();
            local_path_pub_.publish(path);

            return feedback;

        } else {
            const std::vector<SubPath>& all_local_paths = current_config_->local_planner_->getAllLocalPaths();
            if(!all_local_paths.empty()) {
                nav_msgs::Path wpath;
                wpath.header.stamp = ros::Time::now();
                wpath.header.frame_id = current_config_->controller_->getFixedFrame();
                for(const SubPath& path : all_local_paths) {
                    for(const Waypoint& wp : path.wps) {
                        geometry_msgs::PoseStamped pose;
                        pose.pose.position.x = wp.x;
                        pose.pose.position.y = wp.y;
                        pose.pose.orientation = tf::createQuaternionMsgFromYaw(wp.orientation);
                        wpath.poses.push_back(pose);
                    }
                }
                whole_local_path_pub_.publish(wpath);  // 发布完整的局部路径，topic为"whole_local_path"
            }

            is_running_ = execute(feedback, result);
        }
    }

    if(is_running_) {
        return feedback;
    } else {
        return result;
    }
}

整个follower的流程也非常简单，先调用*start()函数复位一些子模块，为local_planner设置GlobalPath和Velocity；接着调用PoseTracker::updateRobotPose()函数获取机器人最新的位姿；调用SupervisorChain::supervise()*函数监视机器人的运行状态；然后local_planner的参数决定是否使用local_planner子模块。

1）不开启local_planner

如果没有开启local_planner，即满足 if(current_config->local_planner->isNull())，则直接进入PathFollower::execute()函数继续执行。PathFollower::execute()的代码如下：

//! 在"PathFollower::update()"函数中被调用，接收上层规划的路径(全局或局部)以后，输出运动控制指令
bool PathFollower::execute(FollowPathFeedback& feedback, FollowPathResult& result)
{
    ROS_ASSERT(current_config_);

    // constants