ROS : Navigation - recovery behavior 源码分析

ROS : Navigation - recovery behavior 源码分析

概述

recovery 机制是当移动机器人认为自己被卡住时,指导机器人进行一系列的恢复行为。

recovery behavior

move_base 节点可以在机器人认为自己被卡住时选择性地执行恢复行为。默认情况下, move_base 节点将采取以下操作来清除空间:

  • 首先,用户指定区域以外的障碍物将从机器人的地图上清除。
  • 接下来,如果可能的话,机器人将进行原地旋转以清除空间。
  • 如果这也失败了,机器人将更积极地清除地图,清除矩形区域之外的所有障碍(在这个区域内机器人可以原地旋转)。这之后将进行另一次原地旋转。
  • 如果所有这些都失败了,机器人将认为它的目标是不可行的,并通知用户它已经中止。
  • 可以使用 recovery_behaviour 参数配置这些恢复行为,并使用 recovery_behavior_enabled 参数禁用这些恢复行为。

导航功能包集合中,有 3 个包与恢复机制有关,这 3 个包中分别定义了 3 个类,都继承了 nav_core 中的接口规范。分别为:

  • clear_costmap_recovery 实现了清除代价地图的恢复行为

    遍历所有层,然后如果某层在可清理列表里就清理掉它的 costmap 。默认可清理列表里只有 obstacle layer ,也就是实时扫描建立的 costmap 。

  • rotate_recovery 实现了旋转的恢复行为, 360 度旋转来尝试清除空间

    转一圈看看有没有路。在 runBehavior 里只需要发指令让小车转一圈,有没有路是 local costmap 在转一圈过程中建立发现的。

  • move_slow_and_clear 实现了缓慢移动的恢复行为

    清理 costmap 然后什么都不管,按照前进速度和转角速度走。从代码里可以看到,根据指定的距离,这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍,然后直接发指令实现的。由于只清除了 obstacle layer ,其实 static layer 的障碍还在,而且这里清不清除跟发指令关系不大,该撞上去的还是会撞的,相当于闭着眼睛往前走。

nav_core::RecoveryBehavior

主要实现两个函数,一个负责初始化,另一个负责执行恢复行为。

什么时候会触发 recovery

move_base 中定义了什么时候会触发 recovery :

enum RecoveryTrigger
{
  PLANNING_R,  // 全局规划失败,即无法算出 plan
  CONTROLLING_R,  // 局部规划失败,即 local planner 无法利用 local costmap 算出一个 local plan ,从而无法得到速度命令
  OSCILLATION_R  // 机器人在不断抖动,长时间被困在一小片区域
};

具体地:

  • 局部规划失败时间大于 controller_patience_ 时会触发恢复机制

    ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);
    
    //check if we've tried to find a valid control for longer than our time limit
    if(ros::Time::now() > attempt_end){
      //we'll move into our obstacle clearing mode
      publishZeroVelocity();
      state_ = CLEARING;
      recovery_trigger_ = CONTROLLING_R;
    }
    
  • 全局规划失败时间大于 planner_patience_ 或者失败次数(从 0 开始)大于 max_planning_retries_ 时会触发恢复机制

    ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
    
    if(runPlanner_ &&
       (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
      //we'll move into our obstacle clearing mode
      state_ = CLEARING;
      runPlanner_ = false;  // proper solution for issue #523
      publishZeroVelocity();
      recovery_trigger_ = PLANNING_R;
    }
    
  • 长时间困在一片小区域时间大于 oscillation_timeout_ 时会触发恢复机制

    //check for an oscillation condition
    if(oscillation_timeout_ > 0.0 &&
        last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
    {
      publishZeroVelocity();
      state_ = CLEARING;
      recovery_trigger_ = OSCILLATION_R;
    }
    

recovery 时会干什么

recovery 时会执行 recovery behavior 。 MoveBase::loadDefaultRecoveryBehaviors 函数会加载一些 default 的 behavior ,基本上两种类型:

源码流程

  • 调用 :恢复机制是在 MoveBase::loadDefaultRecoveryBehaviors 里被调用的,示例如下:

    //first, we'll load a recovery behavior to clear the costmap
    boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery"));
    cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_);
    recovery_behaviors_.push_back(cons_clear);
    
    • 首先,创建恢复机制实例
    • 然后,初始化恢复机制,即 调用恢复机制的 initialize 函数
    • 最后,将该恢复机制加入到恢复机制列表中

    move_base 中加载的默认恢复机制有两种类型,恢复机制列表中共保存 4 个恢复行为:

    • 第一次加载清除代价地图的恢复行为

      初始化节点名称是 ~/conservative_reset ,即 /move_base/conservative_reset ,这里最重要的参数是 指定距离机器人中心正方形边长多少米区域外的代价地图恢复为静态地图 , move_base 中设置 ~/conservative_reset/reset_distance 的默认值为 3.0 3.0 3.0

    • 第一次加载旋转的恢复行为

      初始化节点名称是 ~/rotate_recovery ,即 /move_base/rotate_recovery

    • 第二次加载清除代价地图的恢复行为

      初始化节点名称是 ~/aggressive_reset ,即 /move_base/aggressive_reset ,同样的,这里最重要的参数是 指定距离机器人中心正方形边长多少米区域外的代价地图恢复为静态地图 , move_base 中设置 ~/aggressive_reset/reset_distance 的默认值为 0.46 ∗ 4 = 1.84 0.46 * 4 = 1.84 0.464=1.84

    • 第二次加载旋转的恢复行为

      和第一次加载旋转的恢复行为是一致的

  • 使用 :在 move_base 主程序里,机器人 CLEARING 状态下进行的,这里会 调用恢复机制的 runBehavior 函数

    //we'll try to clear out space with any user-provided recovery behaviors
    case CLEARING:
      ROS_DEBUG_NAMED("move_base","In clearing/recovery state");
      //we'll invoke whatever recovery behavior we're currently on if they're enabled
      if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
        ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
        recovery_behaviors_[recovery_index_]->runBehavior();
    
        //we at least want to give the robot some time to stop oscillating after executing the behavior
        last_oscillation_reset_ = ros::Time::now();
    
        //we'll check if the recovery behavior actually worked
        ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
        last_valid_plan_ = ros::Time::now();
        planning_retries_ = 0;
        state_ = PLANNING;
    
        //update the index of the next recovery behavior that we'll try
        recovery_index_++;
      }
      else{
        ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
        //disable the planner thread
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
        runPlanner_ = false;
        lock.unlock();
    
        ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
    
        if(recovery_trigger_ == CONTROLLING_R){
          ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
          as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
        }
        else if(recovery_trigger_ == PLANNING_R){
          ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
          as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
        }
        else if(recovery_trigger_ == OSCILLATION_R){
          ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
          as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
        }
        resetState();
        return true;
      }
      break;
    
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值