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
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 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& 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(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()"方法获取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 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(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 CollisionAvoiderFactory::makeObstacleAvoider(const std::string &name)
{
if(name == "default_collision_avoider") {
return std::make_shared();
} else {
if (name == "omnidive") {
return std::make_shared();
} else if (name == "ackermann") {
return std::make_shared();
}
}
ROS_WARN_STREAM("No collision_avoider defined with the name '" << name << "'. Defaulting to Omnidrive.");
return std::make_shared();
}
该函数根据传入的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 LocalPlannerFactory::makeConstrainedLocalPlanner(const std::string& name)
{
std::shared_ptr 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(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>> start_config;
DirectionalNode>>::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
PathPlanningAlgorithm;
AStarDynamicSearch是在cslibs_path_planning包的Algorithms.cpp文件中定义，根据宏定义将其展开：
template
class AStarDynamicSearch : public GenericSearchAlgorithm >
{};
可见这是一个定义的模板类，模板参数即
class AStarDynamicSearch : public GenericSearchAlgorithm >
{};
这样就完全定义了路径搜索算法，包括节点的类型、地图的类型、导向函数类型、搜索空间类型等。
接下来的一个步骤是寻找一个路径规划的局部目标点，这里也定义了一个类：
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 = ℘
}
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(frame);
if(path.size() > 0) {
std::vector 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;