Bootstrap

ROS运动规划学习三---move_base

文章目录

前言

一、move_base工程结构

move_base结构如下所示：
在这里插入图片描述

其中move_base.h定义了MoveBase类，该类是实现路径规划与导航的类。

二、move_base简要分析

1.MoveBase类的声明—move_base.h

首先重定义Actionlib的服务 MoveBaseActionServer，并声明MoveBaseState的联合体，表示当前的movebase状态，以及恢复行为原因的联合体，表示引起恢复行为的原因。

  enum MoveBaseState {
    PLANNING, //正处于规划路径
    CONTROLLING, // 正在控制运动
    CLEARING  // 处于恢复或者清除状态，规划失败了或者控制运动失败
  };
  enum RecoveryTrigger
  {
    PLANNING_R,  // 全局规划失败
    CONTROLLING_R, // 局部轨迹规划失败
    OSCILLATION_R // 长时间小范围运动
  };

MoveBase类的成员函数：
构造函数, action构造函数：tf变换的引用

MoveBase(tf2_ros::Buffer& tf);

成员函数：executeCycle，机器人导航到目标点的控制函数，返回值True到达，否则返回false

bool executeCycle(geometry_msgs::PoseStamped& goal);

clearCostmapsService：代价地图清除服务

bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);

MoveBase类的makePlan成员函数，注意与nav_core中的进行区分，goal目标点，plan规划的路径，返回True表示规划成功

bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);

loadRecoveryBehaviors：加载恢复行为函数，返回True表示成功的加载恢复行为

bool loadRecoveryBehaviors(ros::NodeHandle node);

loadDefaultRecoveryBehaviors：加载默认恢复行为

 void loadDefaultRecoveryBehaviors();

clearCostmapWindows：清楚机器人周围窗口内的障碍物。size_x,size_y窗口尺寸大小，和参数clearing_radius有关

void clearCostmapWindows(double size_x, double size_y);

publishZeroVelocity：发布零速度函数
resetState：恢复movebase状态,同时发布零速度，禁止机器人运动

void publishZeroVelocity();
void resetState();

goalCB：接收目标回调函数；
planThread：开辟全局规划线程，进行全局路径规划；
executeCb控制的主要函数，控制机器人底盘的重要函数；
isQuaternionValid：四元数检查函数，检查四元数是否合法，以及长度是否趋于零，并归一化四元数，不合法返回false。
getRobotPose：得到机器人位置
distance：机器人运动距离计算，和参数oscillation_distance比较
goalToGlobalFrame：将目标点转换到全局坐标系

      void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
      void planThread();
      void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
      bool isQuaternionValid(const geometry_msgs::Quaternion& q);
      bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap);
      double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
      geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);

wakePlanner：定时器唤醒

void wakePlanner(const ros::TimerEvent& event);

MoveBase类的成员变量：
之后定义了路径规划以及恢复行为执行过程的一些成员变量、发布接收者、服务等。其中tf_是TF坐标系的变换，as_是导航的服务，actionserver的服务器；

tf2_ros::Buffer& tf_;
MoveBaseActionServer* as_;

tc_ 局部路径规划器实例化后的指针，指向local_planner；代价地图指针：planner_costmap_ros_指向全局代价地图，controller_costmap_ros_指向局部代价地图，planner_全局规划器指针；recovery_behaviors_恢复行为vector，存储全部的恢复行为，一般默认是转圈。这里就和ROS运动规划学习二—nav_core中的nav_core模块对应上。

boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;  
costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;

boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;  
std::string robot_base_frame_, global_frame_;

std::vector<boost::shared_ptr<nav_core::RecoveryBehavior>> recovery_behaviors_; 
std::vector<std::string> recovery_behavior_names_;
unsigned int recovery_index_; // 恢复行为索引

然后以插件的形式实现全局规划器、局部规划器以及恢复行为。在已经注册插件的基础上，插件形式可以实现动态的加载C++类。

//全局规划器
pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
//局部规划器
pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
//恢复行为规划器
pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;

保存规划器中刚刚算出的路径，planner_plan_ 传给latest_plan_ ，作为一个桥梁，在executeCycle函数中传递给controller_plan_

std::vector<geometry_msgs::PoseStamped>* planner_plan_;
std::vector<geometry_msgs::PoseStamped>* latest_plan_;
std::vector<geometry_msgs::PoseStamped>* controller_plan_;

还有其他成员变量，比如用于动态参数服务器的等等。

2.MoveBase类的实现—move_base.cpp

MoveBase实现：构造函数初始化列表，成员变量进行初始化操作：

  MoveBase::MoveBase(tf2_ros::Buffer& tf) :
    tf_(tf),
    as_(NULL),
    planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
    ......
    }

actionserver实例化一个对象，一直执行回调函数executeCb。

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

并加载了move_base的全局参数、make_plan、costmap的参数，和yaml文件中的类似，若yaml文件中没有设置则默认使用这里的参数。

std::string global_planner, local_planner;
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
private_nh.param("planner_frequency", planner_frequency_, 0.0);
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("planner_patience", planner_patience_, 5.0);
private_nh.param("controller_patience", controller_patience_, 15.0);
private_nh.param("max_planning_retries", max_planning_retries_, -1);  
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
private_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);
private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);

private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

新建planner线程，入口函数planThread；

planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

vel_pub_速度下发命令，将速度发送给机器人底盘；current_goal_pub_当前目标点发布指令，发布目标点。

vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );

动作行为发布，目标点以及恢复行为的信息；

ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
recovery_status_pub_= action_nh.advertise<move_base_msgs::RecoveryStatus>("recovery_status", 1);

订阅rviz下发的move_base_simple中的goal话题，获取目标点

ros::NodeHandle simple_nh("move_base_simple");
goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

之后开始定义并初始化全局规划器、局部规划器。

try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}

try {
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}

发布的服务—make_plan，清除代价地图服务clearCostmapsService分别清楚全局代价地图和局部代价地图

    //advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
    //advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

若加载恢复行为没有参数，加载默认的恢复行为，实现在loadDefaultRecoveryBehaviors函数中，加载对应的恢复行为的插件，清除代价地图的恢复行为、原地旋转的恢复行为、主动积极地代价地图重置行为，以及再次原地旋转，没有加载成功出现错误警告。

if(!loadRecoveryBehaviors(private_nh)){
loadDefaultRecoveryBehaviors();
}

初始化结束后，actionserver启动,开始进行路径规划。

void MoveBase::planThread() 调用全局路径规划获取路径，保证运动规划的周期性。因为是另外一个线程，要加锁，首先确认是否要运行全局路径规划器，若需要唤醒或者局部规划器没有运行，则等待。
开始进行规划：获取目标点planner_goal，并赋值给temp_goal；因为在上次循环中加锁，这次解锁。

运行路径规划器，它的主要函数是makePlan，该函数中首先清空路径，判断是否有全局代价地图，能否获取机器人的起始位姿，均满足则调用全局规划器的makePlan函数，也就是继承nav_core中基类的全局规划器，进行规划，若规划器失败或者规划路径为空，则表明规划失败，返回fasle,以上均成功返回True。

planner_plan_->clear();
bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);

若规划器规划出路径则更新相应的全局路径，将planner_plan_放到latest_plan_中，若没有到达目标点，将move_base状态置为 CONTROLLING状态，为了之后的局部路径规划，同时这里会有线程保护。若没有规划出全局路径，同时move_base状态为PLANNING（在executeCycle或者重置状态时设置，一般是刚获得新目标，或者得到路径但计算不出下一步控制，重新进行规划），进入障碍物清楚状态，发布零速度publishZeroVelocity()，恢复行为的原因为PLANNING_R。

若还没达到规划周期则定时器睡眠，在定时器休眠wakePlanner()中通过planner_cond_唤醒。

//规划频率大于零，还没达到下一周期时，休眠时间大于零，需要等待，令wait_for_wake = true,在planThread线程中等待被唤醒
if(planner_frequency_ > 0){
	ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
	if (sleep_time > ros::Duration(0.0)){
		wait_for_wake = true;
		//以固定时间对回调函数进行调用
		timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
	}
}

以上就是planThread()线程的分析，全局路径规划完成以后，进行局部规划的控制，主要执行函数是MoveBase::executeCb（）

void MoveBase::executeCb 程序运行后，回调函数一直刷新，进行局部规划控制，首先判断目标点朝向的四元数是否合法，判断目标点是否有效，并统一转换到全局坐标系 ；发布零速度，有了目标点后开始进行路径规划。加锁，将目标点goal给到全局路径规划planner_goal,令 全局路径标志位 runPlanner_ = true，由于全局规划器线程planThread()有planner_cond_对象的wait函数，这里调用notify唤醒启动全局规划器线程，进行全局规划，并解锁。全局规划完成，就开始进行局部规划，完成导航任务。

 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
      as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
      return;
    }
geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
publishZeroVelocity();

局部规划：判断action_server是否被占用，占用有两种情况：

  • 1 局部规划过程有了新的目标点，如果有了新的目标点，则执行新的目标，把之前的过程重新走一遍：首先判断目标点朝向的四元数是否合法，判断目标点是否有效，并统一转换到全局坐标系，加锁，将目标点goal给到全局路径规划planner_goal,令 全局路径标志位 runPlanner_ = true，由于全局规划器线程planThread()有planner_cond_对象的wait函数，这里调用notify唤醒启动全局规划器线程，进行全局规划，并解锁。然后发布目标点给RVIZ。
if(as_->isNewGoalAvailable()){
	move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
	if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
	as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
	return;
	}
	goal = goalToGlobalFrame(new_goal.target_pose);
	recovery_index_ = 0;
	state_ = PLANNING;
	
    lock.lock();
    planner_goal_ = goal;
    runPlanner_ = true;
    planner_cond_.notify_one();
    lock.unlock();

	ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
	current_goal_pub_.publish(goal);
	
    // 相关的计时变量也要更新
    last_valid_control_ = ros::Time::now();
    last_valid_plan_ = ros::Time::now();
    last_oscillation_reset_ = ros::Time::now();
    planning_retries_ = 0;
	}
  • 2 收到取消任务的命令。重置导航各个部分的状态，执行resetState()函数，把当前的目标点设为占用状态。

若没有被占用或之前已经执行完毕，则开始进行局部规划。先确认目标点坐标系是否和全局坐标系一致，进入planning状态，准备下个执行周期。确认路径规划是唤醒的，发布目标点到RVIZ，相关计时变量更新，也就是和之前的操作差不多。之后，调用executeCycle函数，记录局部控制起始时间（真实时间），导航到目标位置的主要执行函数。

executeCycle() 函数：加锁，定义cmd_vel发布速度命令，获取机器人当前位姿，并发布机器人当前位姿；判断机器人走了足够的距离，重置震荡时间 ，若上次的恢复行为由震荡导致的，重置恢复索引。
move_base状态机切换，控制导航：

  • PLANNING状态，则加锁，唤醒全局路径规划器线程；
  • CONTROLLING状态：判断是否到目标点，若到达，重置导航各部分状态，执行resetState()，并关闭路径规划线程；没有到达，进行震荡判断，防止卡在一定区域，若产生振荡，则发布零速度，状态置为CLEARING，产生恢复行为原因置为OSCILLATION_R；若没有震荡，开始进行局部规划，调用局部规划器的成员函数computeVelocityCommands()，计算出速度，下发给机器人底盘。若局部规划失败，先判断控制命令是否超时，超时，发布零速度，进行障碍物清除恢复行为（move_base状态为CLEARING，产生恢复行为原因为 CONTROLLING_R）；没有超时，但找不到全局路径，重新启动全局路径规划器线程重新规划。
  • CLEARING状态：用提供的恢复行为来清理空间，主要有三个恢复行为，分别调用各自恢复行为的成员函数。若没有恢复成功，则关闭全局规划器结点，打印错误。

总结

本文学习了move_base中的主要函数，用于全局规划线程的函数planThread，局部规划函数executeCb、executeCycle，其它的函数：重置函数resetState、获取机器人位姿函数getRobotPose、计算距离函数distance、目标点转全局坐标函数goalToGlobalFrame、唤醒规划函数wakePlanner 四元数检查函数isQuaternionValid、零速度发布函数publishZeroVelocity等函数作为辅助函数，这里并没有仔细分析。

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
day06-matplotlib不同图形的绘制（柱状图、折线图、饼图）
基于鹈鹕算法优化的二维otsu图像分割
人人都能懂的Vue源码系列(三)—resolveConstructorOptions函数
Docker内存限制
Spring Boot + vue-element 开发个人博客项目实战教程（十三、文章标签功能实现）
鸿蒙测试-常见问题记录
springboot二手车交易系统毕业设计源码131456
throwable和Exception的区别
el-input-number的精度问题
multus-cni之源码解析
;