Gerona中path_follower模块源码解析

Gerona的path_follower模块源码解析

​ GERONA中path_follower包的代码结构如下:
在这里插入图片描述
​ path_follower模块又分为几个子模块:collision_avoidancecontrollerfactorylocal_plannersupervisor,每一个子模块实际都是定义好的一个类。所有程序的入口(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 for return codes
    const bool DONE   = false;
    const bool MOVING = true;

    // Pending error?
    if ( pending_error_ >= 0 ) {
        result.status = pending_error_;
        stop(-1);

        ROS_WARN("pending error");
        return DONE;
    }

    if(path_->empty()) {
        current_config_->controller_->reset();
        result.status = FollowPathResult::RESULT_STATUS_SUCCESS;
        ROS_WARN("no path");
        return DONE;
    }

    // 绘制一个箭头,指示机器人当前的位姿,粉红色
    visualizer_->drawArrow(getFixedFrameId(), 0, pose_tracker_->getRobotPoseMsg(), "slam pose", 2.0, 0.7, 1.0);

    // 输出运动指令,反馈控制状态
    RobotController::ControlStatus status = current_config_->controller_->execute();

    switch(status)
    {
    case RobotController::ControlStatus::REACHED_GOAL:
        if(!current_config_->local_planner_->isNull()) {
            result.status = FollowPathResult::RESULT_STATUS_SUCCESS;
            return DONE;
            //feedback.status = FollowPathFeedback::MOTION_STATUS_OBSTACLE;
            return MOVING;
        } else {
            result.status = FollowPathResult::RESULT_STATUS_SUCCESS;
            return DONE;
        }

    case RobotController::ControlStatus::OBSTACLE:
        if (opt_.abort_if_obstacle_ahead()) {
            // 如果"abort_if_obstacle_ahead"参数为true,遇到障碍物直接终止任务
            result.status = FollowPathResult::RESULT_STATUS_OBSTACLE;
            return DONE;
        } else {
            feedback.status = FollowPathFeedback::MOTION_STATUS_OBSTACLE;
            return MOVING;
        }

    case RobotController::ControlStatus::OKAY:
        feedback.status = FollowPathFeedback::MOTION_STATUS_MOVING;
        return MOVING;

    default:
        //ROS_INFO_STREAM("aborting, status=" << static_cast<int>(status));
        result.status = FollowPathResult::RESULT_STATUS_INTERNAL_ERROR;
        return DONE;
    }
}

该函数中主要是调用controller模块中的*RobotController::execute()*函数,会根据机器人的当前位姿和给定的全局路径计算出具体的控制指令,包括机器人的线速度和角速度(或舵角),并返回执行的状态(REACHED_GOAL、OBSTACLE、OKAY、ERROR),最后还要返回一个FollowPathFeedback和FollowPathResult类型的数据。

2)开启local_planner

​ 如果参数中指定使用local_planner,即不满足 if(current_config->local_planner->isNull()),则要先进行局部规划,规划一个local_path,再执行*RobotController::execute()*函数,同时发布msgs为"local_path"的话题:

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());
}

bool path_search_failure = false;
try {
	// 进行局部规划
	Path::Ptr local_path = current_config_->local_planner_->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);
} catch(const std::runtime_error& e) {
    ROS_ERROR_STREAM("Cannot find local_path: " << e.what());
    path_search_failure = true;
}

2.factory子模块

​ 首先看该模块的文件结构:
在这里插入图片描述
在这里插入图片描述
​ abstract_factory.h中定义了抽象类AbstractFactory,其他几个文件中分别定义了CollisionAvoiderFactory、ControllerFactory、FollowerFactory、LocalPlannerFactory类,他们都继承自AbstractFactory类。作用是构造子模块的实例,因为每一个子模块中根据不同的机器人模型有不同的实现,需要根据提供的参数确定具体实例化哪一个派生类。在PathFollower的构造函数中构造了一个FollowerFactory类型的实例,FollowerFactory的构造函数定义如下:

FollowerFactory::FollowerFactory(PathFollower &follower)
    : opt_(follower.getOptions()),  // "opt_"实例(对象)由"PathFollowerParameters::getInstance()"函数构造
      follower_(follower),
      pose_tracker_(follower.getPoseTracker()),
      // PathFollowerParameters::getInstance()函数中获取launch文件中的param,并用于构造ControllerFactory实例,以共享指针的形式赋给controller_factory_变量
      controller_factory_(new ControllerFactory(*PathFollowerParameters::getInstance())),
      local_planner_factory_(new LocalPlannerFactory(*LocalPlannerParameters::getInstance())),
      collision_avoider_factory_(new CollisionAvoiderFactory)
{
}

在这个构造函数的初始化列表中又分别创建了ControllerFactory、LocalPlannerFactory和CollisionAvoiderFactory类型的三个实例。其中*PathFollowerParameters::getInstance()*函数中会构建PathFollowerParameters类型的实例,获取到参数服务器中的相关参数:

PathFollowerParameters():
    // 下面的都是构造函数的参数列表
    // P是一个定义在"parameters.h"中的模板类,其构造函数中会调用"ros::param::param<T>()"方法获取ros参数服务器中的参数
    controller(this, "controller_type", "ackermann_purepursuit", "Defines which controller is used."),
    collision_avoider(this, "collision_avoider", "", "Defines, which collisison avoider is used."),

    world_frame(this, "world_frame",
                nh.param("gerona/world_frame", std::string("map")),
                "Name of the world frame."),
    robot_frame(this, "robot_frame",
                nh.param("gerona/robot_frame", std::string("base_link")),
                "Name of the robot frame."),
    odom_frame(this, "odom_frame",
               nh.param("gerona/odom_frame", std::string("odom")),
               "Name of the odometry frame."),

    // 下面的参数在follower.launch文件中定义
    wp_tolerance(this, "waypoint_tolerance",  0.20 , ""),
    goal_tolerance(this, "goal_tolerance",  0.15 , ""),
    steer_slow_threshold(this, "steer_slow_threshold",  0.25 ,
                         "Robot slows down, when steering angle exceeds this threshold."
                         " May not be supported by all robot controllers."),
    min_velocity(this, "min_velocity",  0.4 ,
                 "Minimum speed of the robot (needed, as the outdoor buggys can't handle"
                 " velocities below about 0.3)."),
    max_velocity(this, "max_velocity",  2.0 ,
                 "Maximum velocity (to prevent the high level control from running amok)."),

    abort_if_obstacle_ahead(this, "abort_if_obstacle_ahead",  false,
                            "If set to true, path execution is aborted, if an obstacle is"
                            " detected on front of the robot. If false, the robot will"
                            " stop, but not abort (the obstacle might move away).")
{
}

*LocalPlannerParameters::getInstance()*函数则获取与局部规划相关的参数。

​ 当path_control节点向path_follower节点请求服务时,进入*PathFollowerServer::followPathGoalCB()回调函数,回调函数中进一步执行PathFollower::setGoal()*函数:

void PathFollower::setGoal(const FollowPathGoal &goal)
{    
    // Choose robot controller
    // 从"follow_path"消息(由path_controller_node发布)中提取路径跟随的配置参数(包括跟随使用的方法)
    // 此处开始将上城传来的配置参数赋给变量current_config_
    PathFollowerConfigName config_name = goalToConfig(goal);

    auto pos = config_cache_.find(config_name);
    if(pos != config_cache_.end()) {
        current_config_ = pos->second;
    }
    else {
        // 构造一个RobotController(基类)类型、一个AbstractLocalPlanner类型、一个CollisionAvoider类型的实例
        current_config_ = follower_factory_->construct(config_name);
        config_cache_[config_name] = current_config_;
    }

    ROS_ASSERT(current_config_);
    if(obstacle_cloud_) {
        current_config_->collision_avoider_->setObstacles(obstacle_cloud_);
    }

    vel_ = goal.follower_options.velocity;
    current_config_->controller_->setVelocity(vel_);

    pending_error_ = -1;

    if (goal.path.paths.empty()) {
        ROS_ERROR("Got an invalid path.");
        stop(FollowPathResult::RESULT_STATUS_INTERNAL_ERROR);
        return;
    }

    if(is_running_) {
        if(goal.follower_options.init_mode != FollowerOptions::INIT_MODE_CONTINUE) {
            ROS_ERROR("got a new goal, stopping");
            stop(FollowPathResult::RESULT_STATUS_SUCCESS);
        }
        is_running_ = false;
    }

    setPath(goal.path);

    supervisors_->notifyNewGoal();

    ROS_INFO_STREAM("Following path with " << goal.path.paths.size() << " segments.");

    ROS_INFO_STREAM("using follower configuration:\n- controller: " << config_name.controller <<
                    "\n- avoider: " << typeid(*current_config_->collision_avoider_).name() <<
                    "\n- local planner: " << config_name.local_planner);
}

该函数中执行*FollowerFactory::construct()*函数根据传入的配置开始分别构造RobotController类、 AbstractLocalPlanner类、CollisionAvoider类的实例:

std::shared_ptr<PathFollowerConfig> FollowerFactory::construct(const PathFollowerConfigName &config)
{
    ROS_ASSERT_MSG(!config.controller.empty(), "No controller specified");
    ROS_ASSERT_MSG(!config.local_planner.empty(), "No local planner specified");
    //ROS_ASSERT_MSG(!config.collision_avoider.empty(), "No obstacle avoider specified");

    PathFollowerConfig result;
    // 根据参数中指定的controller名字开始构造不同的实例(分配内存),以共享指针的形式记录
    result.controller_ = controller_factory_->makeController(config.controller);
    result.local_planner_ = local_planner_factory_->makeConstrainedLocalPlanner(config.local_planner);

    std::string collision_avoider = config.collision_avoider;
    if(collision_avoider.empty()) {
        // 使用默认的CollisionAvoider
        collision_avoider = controller_factory_->getDefaultCollisionAvoider(config.controller);
        ROS_ASSERT_MSG(!collision_avoider.empty(), "No obstacle avoider specified");
    }
    // 与controller_和local_planner_不同,这里是调用"std::make_shared"方法分配内存(CollisionDetectorAckermann或CollisionDetectorOmnidrive类型的实例)
    result.collision_avoider_ = collision_avoider_factory_->makeObstacleAvoider(collision_avoider);

    ROS_ASSERT_MSG(result.controller_ != nullptr, "Controller was not set");
    ROS_ASSERT_MSG(result.local_planner_ != nullptr, "Local Planner was not set");
    ROS_ASSERT_MSG(result.collision_avoider_ != nullptr, "Obstacle Avoider was not set");

    // wiring
    result.collision_avoider_->setTransformListener(&pose_tracker_.getTransformListener());
    result.collision_avoider_->setRobotFrameId(pose_tracker_.getRobotFrameId());

    result.local_planner_->init(result.controller_.get(), &pose_tracker_);

    result.controller_->init(&pose_tracker_, result.collision_avoider_.get());

    pose_tracker_.setLocal(!result.local_planner_->isNull());

    return std::make_shared<PathFollowerConfig>(result);
}

至此controller、local_planner和collision_avoider子模块被构造完毕。

3.collision_avoider子模块

​ 文件结构如下:
在这里插入图片描述
​ collision_avoider.cpp中定义的CollisionAvoider类是所有类的基类,往下派生是CollisionDetector类,再往下是CollisionDetectorPolygon类,最后是CollisionDetectorAckermann类或CollisionDetectorOmnidrive类。

​ 在*FollowerFactory::construct()*函数中执行CollisionAvoiderFactory::makeObstacleAvoider()函数构造CollisionAvoider类的实例:

std::shared_ptr<CollisionAvoider> CollisionAvoiderFactory::makeObstacleAvoider(const std::string &name)
{
    if(name == "default_collision_avoider") {
        return std::make_shared<CollisionDetectorOmnidrive>();
    } else {
        if (name == "omnidive") {
            return std::make_shared<CollisionDetectorOmnidrive>();
        } else if (name == "ackermann") {
            return std::make_shared<CollisionDetectorAckermann>();
        }
    }
    ROS_WARN_STREAM("No collision_avoider defined with the name '" << name << "'. Defaulting to Omnidrive.");
    return std::make_shared<CollisionDetectorOmnidrive>();
}

该函数根据传入的string参数选择构造CollisionDetectorOmnidrive类还是CollisionDetectorAckermann类的实例,返回构造对象的共享指针。

​ *CollisionDetectorAckermann::avoid()函数在RobotController::execute()*函数中执行,其中再调用CollisionDetector::avoid()函数,主要功能是以机器人为中心创建一个多边形的冲突检测框,如果框的范围内出现障碍物,立即将机器人的速度设置为0:

bool CollisionDetector::avoid(MoveCommand * const cmd,
                             const CollisionAvoider::State &state)
{
    if (externalError_ !=  0)
    {
        cmd->setVelocity(0);
        if (cmd->canRotate()) cmd->setRotationalVelocity(0);
        return true;
    }
    // 航向
    float course = cmd->getDirectionAngle(); //TODO: use CoursePredictor instead of command?

    //! Factor which defines, how much the box is enlarged in curves.
    // 这个参数决定了根据舵角扩大冲突框边长的程度
    const float enlarge_factor = 0.3; // should this be a parameter? 0.5

    /* Calculate length of the collision box, depending on current velocity.
     * v <= v_min:
     *   length = min_length
     * v > v_min && v < v_sat:
     *   length  interpolated between min_length and max_length:
     *   length = min_length + FACTOR * (max_length - min_length) * (v - v_min) / (v_sat - v_min)
     * v >= v_sat:
     *   length = max_length
     */
    float v = cmd->getVelocity();

    const float diff_to_min_velocity = v - state.parameters.min_velocity();

    float vel_saturation = opt_.velocity_saturation() > 0
            ? opt_.velocity_saturation()
            : state.parameters.max_velocity();
    const float norm = vel_saturation - state.parameters.min_velocity();
    const float span = opt_.max_length() - opt_.min_length();
    const float interp = std::max(0.0f, diff_to_min_velocity) / std::max(norm, 0.001f);
    const float f = std::min(1.0f, opt_.velocity_factor() * interp);

    float box_length = opt_.min_length() + span * f;

    //ROS_DEBUG_NAMED(MODULE, "Collision Box: v = %g -> len = %g", v, box_length);

    double distance_to_goal = state.path->getCurrentSubPath().wps.back().distanceTo(state.path->getCurrentWaypoint());

    if(box_length > distance_to_goal) {
        box_length = distance_to_goal + 0.2;
    }

    if(box_length < opt_.crit_length()) {
        box_length = opt_.crit_length();
    }


    bool collision = false;
    //if(obstacles_ && !obstacles_->empty())
    if(obstacles_)
    {
        // 根据车当前的速度和方向绘制一个多边形框,检查是否与障碍物点云相重叠
        collision = checkOnCloud(obstacles_, opt_.width(),
                                      box_length, course, enlarge_factor);
    } else if (!obstacles_) {
        ROS_WARN_THROTTLE(1, "no obstacle cloud is available");
    }


    if(collision) {
        // stop motion
        cmd->setVelocity(0);
    }
    return collision;
}

实际这个多边形框会根据机器人的速度、舵角改变,以达到一定的预测碰撞效果。

4.controller子模块

​ 文件结构如下:
在这里插入图片描述
其中robotcontroller.cpp中定义了其他控制算法的基类RobotController类,其他cpp文件是各个针对不同机器人模型编写的控制算法。RobotController类的成员函数大多是虚函数,需要在继承类中实现。

​ *RobotController::execute()函数在PathFollower::execute()*函数中被调用,是控制算法的入口:

RobotController::ControlStatus RobotController::execute()
{
    if(!path_) {
       return ControlStatus::ERROR;
    }

    // 发布"visualization_marker"话题,ns = "robot path"
    publishPathMarker();

    MoveCommand cmd;
    MoveCommandStatus status = computeMoveCommand(&cmd);


    if (status != MoveCommandStatus::OKAY) {
        stopMotion();
        return MCS2CS(status);
    } else {
        // PathFollowerParameters::getInstance()获取ros参数服务器中的参数(定义在follower.launch文件中)
        // 返回一个PathFollowerParameters类型的指针,包含launch文件中的所有参数
        CollisionAvoider::State state(path_, *PathFollowerParameters::getInstance());
        bool cmd_modified = collision_avoider_->avoid(&cmd, state);

        if (!cmd.isValid()) {
            ROS_ERROR("Invalid move command.");
            stopMotion();
            return ControlStatus::ERROR;
        } else {
            // 根据不同的运动模型调用不同的控制函数,发布"cmd_vel"话题
            publishMoveCommand(cmd);
            return cmd_modified ? ControlStatus::OBSTACLE : ControlStatus::OKAY;
        }
    }
}

其中执行了computeMoveCommand()函数,该函数是具体定义在每一个子类中的,例如Robotcontroller_Ackermann_PurePursuit::computeMoveCommand():

RobotController::MoveCommandStatus Robotcontroller_Ackermann_PurePursuit::computeMoveCommand(
      MoveCommand* cmd) {

   if(path_interpol.n() <= 2)
      return RobotController::MoveCommandStatus::ERROR;

    Eigen::Vector3d pose = pose_tracker_->getRobotPose();

    RobotController::findOrthogonalProjection();

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

   double lookahead_distance = velocity_;
   if(getDirSign() >= 0.)
      lookahead_distance *= params_.factor_lookahead_distance_forward();
   else
      lookahead_distance *= params_.factor_lookahead_distance_backward();

   // angle between vehicle theta and the connection between the rear axis and the look ahead point
   const double alpha = computeAlpha(lookahead_distance, pose);

   // 计算前轮的转角(参考博客https://blog.csdn.net/AdamShan/article/details/80555174)
   // 注意到一点,在这种算法中,车体的中心应该是后轮的中心。 vehicle_length为车辆的轴距
   const double delta = atan2(2. * params_.vehicle_length() * sin(alpha), lookahead_distance);

    double exp_factor = RobotController::exponentialSpeedControl();
   move_cmd_.setDirection(params_.factor_steering_angle() * (float) delta);
    double v = getDirSign() * (float) velocity_ * exp_factor;
    move_cmd_.setVelocity(v);


   *cmd = move_cmd_;

   return RobotController::MoveCommandStatus::OKAY;
}

purepursuit是一种针对ackermann模型车辆的控制算法,以车辆后轮轴中心为控制点,将该控制点投影到要跟踪的路径上,在路径上找一个前瞻点,以此计算出车辆应该执行的舵角和速度,返回MoveCommand类型的变量,最终保证机器人可以完全跟踪该路径运行。

​ *PathFollower::execute()函数中还执行了CollisionDetectorAckermann::avoid()函数,根据激光雷达的数据检查机器人车身与周围障碍物的碰撞情况,如果发生碰撞或预测到会发生碰撞,则立即刹车。最后执行publishMoveCommand()*函数将MoveCommand类型的变量转换为geometry_msgs::Twist类型的消息,以"cmd_vel"的名字发布。

5.local_planner子模块

​ local_planner子模块中包含了多种类型的局部规划算法,但是gerona开源的源码中仅实现了两种算法,一个是LocalPlannerAStar,另一个是LocalPlannerModel,对应在launch文件中的“path_follower/local_planner/algorithm”参数分别是”Astar“和”HS_Model“。代码结构如下:
在这里插入图片描述
它包含了high_speed、low_speed、model_based三种类型的算法,其中high_speed类型的方法全都是不完整的,无法直接使用。abstract_local_planner.cpp中定义了AbstractLocalPlanner类,这是一个抽象类,其中成员函数被定义为虚函数或纯虚函数;local_planner_astar.cpp中定义了LocalPlannerAStar类,继承自AbstractLocalPlanner类,是AbstractLocalPlanner抽象类的一种实现;local_planner_model.cpp中定义的LocalPlannerModel也是AbstractLocalPlanner抽象类的一种实现;另外是local_planner_null.cpp文件,其中定义了LocalPlannerNull类,当launch文件中“path_follower/local_planner/algorithm”参数是”Null“时,会实例化这个类。

​ AbstractLocalPlanner的派生类在LocalPlannerFactory::makeConstrainedLocalPlanner()函数中实例化:

std::shared_ptr<AbstractLocalPlanner> LocalPlannerFactory::makeConstrainedLocalPlanner(const std::string& name)
{
    std::shared_ptr<AbstractLocalPlanner> local_planner = makeLocalPlanner(name);

    //Begin Constraints and Scorers Construction
    if(opt_.use_distance_to_path_constraint()){
        local_planner->addConstraint(Dis2Path_Constraint::Ptr(new Dis2Path_Constraint));
    }

    if(opt_.use_distance_to_obstacle_constraint()){
        local_planner->addConstraint(Dis2Obst_Constraint::Ptr(new Dis2Obst_Constraint));
    }

    if(opt_.score_weight_distance_to_path() != 0.0){
        local_planner->addScorer(Dis2PathP_Scorer::Ptr(new Dis2PathP_Scorer),
                                 opt_.score_weight_distance_to_path());
    }

    if(opt_.score_weight_delta_distance_to_path() != 0.0){
        local_planner->addScorer(Dis2PathD_Scorer::Ptr(new Dis2PathD_Scorer),
                                 opt_.score_weight_delta_distance_to_path());
    }

    if(opt_.score_weight_curvature() != 0.0){
        local_planner->addScorer(Curvature_Scorer::Ptr(new Curvature_Scorer),
                                 opt_.score_weight_curvature());
    }

    if(opt_.score_weight_delta_curvature() != 0.0){
        local_planner->addScorer(CurvatureD_Scorer::Ptr(new CurvatureD_Scorer),
                                 opt_.score_weight_delta_curvature());
    }

    if(opt_.score_weight_level() != 0.0){
        local_planner->addScorer(Level_Scorer::Ptr(new Level_Scorer),
                                 opt_.score_weight_level());
    }

    if(opt_.score_weight_distance_to_obstacles() != 0.0){
        local_planner->addScorer(Dis2Obst_Scorer::Ptr(new Dis2Obst_Scorer),
                                 opt_.score_weight_distance_to_obstacles());
    }

    return local_planner;
}

其中makeLocalPlanner()函数根据传入的参数构建相应规划算法的实例,值得一提的是,这里用到了ROS中一个pluginlib库,它是一个可以动态加载的扩展功能类。

​ 如果启用了局部规划,在pathfollower.cpp文件的PathFollower::update()函数中将调用AbstractLocalPlanner::updateLocalPath(),以Astar方法为例,实际执行的是*LocalPlannerAStar::updateLocalPath()*函数,它是AbstractLocalPlanner抽象类中接口函数的具体实现:

Path::Ptr LocalPlannerAStar::updateLocalPath()
{
    ros::Time now = ros::Time::now();

//    update_interval_ = ros::Duration(0.1);

    // only calculate a new local path, if enough time has passed.
    if(last_update_ + update_interval_ < now) {
        if(global_path_.n() == 0) {
            // 设置超时1秒就退出
            ROS_WARN("cannot calculate local path, global path is empty");
            return nullptr;
        }
        return doUpdateLocalPath();

    } else {
        return nullptr;
    }
}

在这里对局部规划的更新周期做了一个限制,“update_interval_”参数在launch文件中指定。实际执行的规划函数*doUpdateLocalPath()*如下:

Path::Ptr LocalPlannerAStar::doUpdateLocalPath()
{
    ros::Time now = ros::Time::now();

    std::string world_frame = PathFollowerParameters::getInstance()->world_frame();
    std::string odom_frame = PathFollowerParameters::getInstance()->odom_frame();

    // transform the waypoints from world to odom
    Eigen::Vector3d pose = pose_tracker_->getRobotPose();
    int n = global_path_.n();
    // 当前pose到终点的距离
    double dist_to_last_pt = std::hypot(pose(0) - global_path_.p(n-1),
                                        pose(1) - global_path_.q(n-1));

    // find a path that starts at the robot's pose
    Path::Ptr local_wps = std::make_shared<Path>(odom_frame);

    // 更新地图,构建local_map
    updateMap();
    map_info = map_info_static;

    // 构建local_map,将障碍物叠加上去
    if(!integrateObstacles()) {
        return {};
    }

    DynamicSteeringNeighborhood::goal_angle_threshold = pnh_.param("planner/goal_angle_threshold", 15.0) / 180. * M_PI;

    DynamicSteeringNeighborhood::allow_forward = true;
    DynamicSteeringNeighborhood::allow_backward = true;

    DynamicSteeringNeighborhood::MAX_STEER_ANGLE = pnh_.param("planner/ackermann_max_steer_angle", 45);
    DynamicSteeringNeighborhood::STEER_DELTA = pnh_.param("planner/ackermann_steer_delta", 15);
    DynamicSteeringNeighborhood::steer_steps = pnh_.param("planner/ackermann_steer_steps", 2);
    DynamicSteeringNeighborhood::LA = pnh_.param("planner/ackermann_la", 0.6);

    if(dist_to_last_pt > 2 * DynamicSteeringNeighborhood::LA) {
        DynamicSteeringNeighborhood::goal_dist_threshold = pnh_.param("planner/goal_dist_threshold", 0.25);
    } else {
        DynamicSteeringNeighborhood::goal_dist_threshold = pnh_.param("planner/goal_dist_threshold", 0.5);
    }

    // 到终点的距离小于4倍LA参数
    bool final_approach = dist_to_last_pt < 4 * DynamicSteeringNeighborhood::LA;

    // 开始搜索局部路径
    try {
        if(final_approach) {
            DynamicSteeringNeighborhood::reversed = true;
            local_wps = calculateFinalAvoidingPath(odom_frame);
        } else {
            DynamicSteeringNeighborhood::reversed = false;
            local_wps = calculateAvoidingPath(odom_frame);
        }

    } catch(const std::runtime_error& e) {
        local_map_pub_.publish(local_map);

        ROS_ERROR_STREAM_THROTTLE(1, "planning failed: " << e.what());
        return {};
    }

    // 发布topic为"local_map"的地图
    local_map_pub_.publish(local_map);

    if(local_wps->empty()) {
        return {};
    }

    //! 将规划的局部路径传递给controller
    setPath(local_wps, now);

    return local_wps;
}

这里比较奇怪的一点是它要先获取map到odom坐标系的变换关系,把path转换到odom坐标系下,我认为是没有这个必要的,所以我们的odom_frame也设置为了“map”,就不存在这个转换的过程。整个局部规划的流程也很简单,与全局规划相似。首先是创建用于局部规划的地图,叠加传感器探测到的动态障碍物信息,然后计算与全局目标点的距离,根据距离选择不同的规划策略。

距离目标点较远时,执行*LocalPlannerAStar::calculateAvoidingPath()*函数:
Path::Ptr LocalPlannerAStar::calculateAvoidingPath(const std::string &frame)
{
    DirectionalNode<SteeringNode<HeuristicNode<Pose2d>>> start_config;
    DirectionalNode<SteeringNode<HeuristicNode<Pose2d>>>::init(start_config, start_config);

    // start at the robot pose
    Eigen::Vector3d odom_pose_v = pose_tracker_->getRobotPose();
    tf::Pose odom_pose(tf::createQuaternionFromYaw(odom_pose_v(2)),
                       tf::Vector3(odom_pose_v(0), odom_pose_v(1), 0.0));

    Pose2d pose_cell = convertToCell(map_info.get(), odom_pose);

    start_config.x = pose_cell.x;
    start_config.y = pose_cell.y;
    start_config.theta = pose_cell.theta;

    start_config.steering_angle = 0; // TODO

    PathPlanningAlgorithm algo;
    algo.setMap(map_info.get());
    algo.setTimeLimit(1.0);

    Stopwatch sw;
    sw.restart();

    algo.setPathCandidateCallback([this](const typename PathPlanningAlgorithm::PathT& path) {
        return false;
    });

    //! A path is sequence of waypoints.
    //! 这里做了类型转换,global_path_是PathInterpolated类型,
    //! 其中定义了"operator SubPath()"函数,就可以将PathInterpolated类型转换为SubPath类型
    SubPath map_path = global_path_;

    //! 确定引导目标点,即局部规划的目标点。
    NearPathTest goal_test_forward(*this, map_path, algo, start_config, map, local_map, map_info.get());
    Pose2d hgoal = *goal_test_forward.getHeuristicGoal();  // 做局部规划的目标点

    geometry_msgs::Pose arrow;
    map_info->cell2pointSubPixel(hgoal.x, hgoal.y, arrow.position.x, arrow.position.y);
    arrow.position.z = 0;
    arrow.orientation = tf::createQuaternionMsgFromYaw(hgoal.theta);
    // 画一个"ARROW"类型的marker,以"visualization_marker"话题发布
    Visualizer::getInstance()->drawArrow("map", 0, arrow, "heuristic_goal", 1.0, 1.0, 0.0, 0.0);

    SearchOptions search_options;
    search_options.penalty_backward = 1.0;// 1.1;
    search_options.penalty_turn = 5.0;//5.0;
    search_options.oversearch_distance = 0.0;

    sw.restart();
    typename PathPlanningAlgorithm::PathT path;
    try {
        //! 开始搜索路径
        path = algo.findPathWithStartConfiguration(start_config, goal_test_forward, search_options);
        ROS_INFO("findPathWithStartConfiguration.size() = %d", path.size());
    } catch(const std::exception& e) {
        ROS_ERROR_STREAM("path search failed: " << e.what());
        throw std::runtime_error("found no local path");
    }
    ROS_INFO_STREAM("LocalPlanner earch took " << sw.usElapsed()/1000.0 << "ms for " << algo.getExpansions() << " expansions (" << algo.getMultiExpansions() << " multiply expanded and " << algo.getTouchedNodes() << " touched nodes).");

    if(path.empty()) {
        ROS_ERROR("found no local path");
        throw std::runtime_error("found no local path");
    }

    //! 将上面搜索出的PathPlanningAlgorithm::PathT类型的路径经过平滑插值后转换为Path::Ptr类型返回
    return convertPath(path, map_info.get(), frame);
}

这里需要特别介绍一下PathPlanningAlgorithm algo这个类型,PathPlanningAlgorithm的实际定义是:

typedef
AStarDynamicSearch<DynamicSteeringNeighborhood, NoExpansion, Pose2d, GridMap2d, 500 >
PathPlanningAlgorithm;

AStarDynamicSearch是在cslibs_path_planning包的Algorithms.cpp文件中定义,根据宏定义将其展开:

template <class Neighborhood, class AnalyticExpansion = NoExpansion, class PointT = Pose2d, class MapT = GridMap2d, int DrawInterval=0, int DrawStartInterval=0> 
class AStarDynamicSearch : public GenericSearchAlgorithm<GenericParameter<PointT,HeuristicL2,MapT,Neighborhood,AnalyticExpansion,PriorityQueueManager,DynamicDirectionalStateSpaceManager,DrawInterval, DrawStartInterval> >
{};

可见这是一个定义的模板类,模板参数即<DynamicSteeringNeighborhood, NoExpansion, Pose2d, GridMap2d, 500>,所以最终的类定义为:

class AStarDynamicSearch : public GenericSearchAlgorithm <GenericParameter<Pose2d,HeuristicL2,GridMap2d,DynamicSteeringNeighborhood,NoExpansion,PriorityQueueManager,DynamicDirectionalStateSpaceManager,0,0> >
{};

这样就完全定义了路径搜索算法,包括节点的类型、地图的类型、导向函数类型、搜索空间类型等。

​ 接下来的一个步骤是寻找一个路径规划的局部目标点,这里也定义了一个类:

struct NearPathTest
{
    NearPathTest(const LocalPlannerAStar& parent, const SubPath& odom_path,
                 PathPlanningAlgorithm& algo, const Pose2d& start_cell, const nav_msgs::OccupancyGrid& map,
                 nav_msgs::OccupancyGrid& local_map, const SimpleGridMap2d* map_info)
        : parent(parent), odom_path(odom_path),
          algo(algo), map(map), local_map(local_map), map_info(map_info),
          res(map.info.resolution),
          ox(map.info.origin.position.x),
          oy(map.info.origin.position.y),
          w(map.info.width),
          h(map.info.height),
          start_cell(start_cell),
          candidates(0)
    {
        min_dist = DynamicSteeringNeighborhood::LA;
        desired_dist = 2.0 * DynamicSteeringNeighborhood::LA;

        updateHeuristicGoal();
    }

    //! 确定一个引导目标点,即局部规划的目标点。
    void updateHeuristicGoal()
    {
        if(odom_path.empty()) {
            return;
        }

        double mx,my;
        map_info->cell2pointSubPixel(start_cell.x, start_cell.y, mx, my);
        Eigen::Vector3d start_pose(mx,my, start_cell.theta);

        // 在路径上找一个距离当前位置最近的节点
        std::size_t start_index = findClosestWaypoint(start_pose, odom_path);

        double dist_accum = 0.0;
        const Waypoint* last_wp = &odom_path.at(start_index);

        for(std::size_t i = start_index, n = odom_path.size(); i < n; ++i) {
            const Waypoint& wp = odom_path[i];
            dist_accum += wp.distanceTo(*last_wp);  // 从当前节点开始向后累加路径长度

            // desired_dist = 2.0 * DynamicSteeringNeighborhood::LA
            if(dist_accum >= desired_dist * 2) {
                // 累计长度大于4倍LA值,则确定引导目标点(局部规划的目标点)
                map_info->point2cellSubPixel(wp.x, wp.y, heuristic_goal.x,heuristic_goal.y);
                heuristic_goal.theta = wp.orientation;
                return;
            }

            last_wp = &wp;
        }

        const Waypoint& wp = odom_path.back();
        map_info->point2cellSubPixel(wp.x, wp.y, heuristic_goal.x,heuristic_goal.y);
        heuristic_goal.theta = wp.orientation;
    }
    
    ...
    const Pose2d* getHeuristicGoal() const
    {
        return &heuristic_goal;
    }
    ...
}

在*NearPathTest::updateHeuristicGoal()*函数中,首先把当前位置投影到全局的waypoints上,然后从当前point开始向后累加路径的长度,当累计长度大于4倍LA值时,则确定引导目标点(局部规划的目标点)。

​ 获得局部目标点后,则开始搜索局部路径,执行函数algo.findPathWithStartConfiguration(start_config, goal_test_forward, search_options);,该函数定义在cslibs_path_planning包的SearchAlgorithm.hpp文件中,所以实际上搜索算法都是在cslibs_path_planning包中实现的,这里只是做了一个封装。最后的最后,执行函数convertPath():

Path::Ptr convertPath(typename PathPlanningAlgorithm::PathT path,
                      lib_path::CollisionGridMap2d* map_info,
                      const std::string &frame)
{
    Path::Ptr avoiding_path_map = std::make_shared<Path>(frame);

    if(path.size() > 0) {
        std::vector<SubPath> paths;

        // insert a first path
        paths.emplace_back();
        SubPath* current_subpath = &paths.back();
        current_subpath->forward = path.front().forward;

        Waypoint last_wp_map;
        for (auto pt : path) {
            Waypoint wp_map;
            map_info->cell2pointSubPixel(pt.x, pt.y, wp_map.x, wp_map.y);
            wp_map.orientation = pt.theta;

            if(pt.forward != current_subpath->forward) {

                // when the direction changes: add a new path segment
                if(!current_subpath->empty()) {
                    paths.emplace_back();
                    current_subpath = &paths.back();
                }
                current_subpath->forward = pt.forward;

                current_subpath->push_back(last_wp_map);
            }

            current_subpath->push_back(wp_map);

            last_wp_map = wp_map;
        }

        for(SubPath& subpath : paths) {
            // 对搜索出的局部路径进行插值和平滑处理,最终路径的分辨率(两个waypoints之间的距离)为0.1m
            AbstractLocalPlanner::smoothAndInterpolate(subpath);
        }

//        ROS_INFO("smoothAndInterpolate path size = %d", current_subpath->size());
        avoiding_path_map->setPath(paths);
    }

    return avoiding_path_map;
}

主要功能是对搜索的路径进行插值和平滑处理,最终路径的分辨率为0.1m,*AbstractLocalPlanner::smoothAndInterpolate(subpath)*代码如下:

void AbstractLocalPlanner::smoothAndInterpolate(SubPath& local_wps){
    //interpolate
    local_wps = interpolatePath(local_wps, 0.5);
    //smoothing
    local_wps = smoothPath(local_wps, 0.6, 0.15);
    //final interpolate
    local_wps = interpolatePath(local_wps, 0.1);
    //final smoothing
    local_wps = smoothPath(local_wps, 2.0, 0.4);
}
当距离终点较近时,执行*LocalPlannerAStar::calculateFinalAvoidingPath()*函数

​ 该函数与*LocalPlannerAStar::calculateAvoidingPath()*的内容基本是一致的,只是由于接近目标点了,所以直接把局部目标点设置为全局目标点:

// end at the last path pose
int n = global_path_.n();
ROS_ASSERT(n > 1);
double theta = global_path_.theta_p(n-1);
theta = std::atan2(global_path_.q(n-1) - global_path_.q(n-2),
                   global_path_.p(n-1) - global_path_.p(n-2));
tf::Pose last_pose (tf::createQuaternionFromYaw(theta),
                    tf::Vector3(global_path_.p(n-1), global_path_.q(n-1), 0.0));
Pose2d goal_cell = convertToCell(map_info.get(), last_pose);

goal_config.x = goal_cell.x;
goal_config.y = goal_cell.y;
goal_config.theta = goal_cell.theta;
  • 2
    点赞
  • 13
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值