ROS move_base.cpp源码分析（二）MoveBase::reconfigureCB

动态参数服务器回调函数reconfigureCB

void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
     
    boost::recursive_mutex::scoped_lock l(configuration_mutex_);//动态更改节点参数的回调函数，用于修改参数
    //一旦被调用，我们要确保有原始配置
    if(!setup_)
    {
     
      last_config_ = config;
      default_config_ = config;
      setup_ = true;
      return;
    }

    if(config.restore_defaults) {
     
      config = default_config_;
      //如果有人在参数服务器上设置默认值，要防止循环
      config.restore_defaults = false;
    }

    if(planner_frequency_ != config.planner_frequency)
    {
     
      planner_frequency_ = config.planner_frequency;
      p_freq_change_ = true;
    }

    if(controller_frequency_ != config.controller_frequency)
    {
     
      controller_frequency_ = config.controller_frequency;
      c_freq_change_ = true;
    }

    planner_patience_ = config.planner_patience;
    controller_patience_ = config.controller_patience;
    max_planning_retries_ = config.max_planning_retries;
    conservative_reset_dist_ = config.conservative_reset_dist;

    recovery_behavior_enabled_ = config.recovery_behavior_enabled;
    clearing_rotation_allowed_ = config.clearing_rotation_allowed;
    shutdown_costmaps_ = config.shutdown_costmaps;

    oscillation_timeout_ = config.oscillation_timeout;
    oscillation_distance_ = config.oscillation_distance;
    if(config.base_global_planner != last_config_.base_global_planner) {
     
      boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
      //创建全局规划 
      ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
      try {
     
        planner_ = bgp_loader_.createInstance(config.base_global_planner);

        //等待当前规划结束
        boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);

        //在新规划开始之前清除旧的
        planner_plan_->clear();
        latest_plan_->clear();
        controller_plan_->clear();
        resetState();
        planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);

        lock.unlock();
      } 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", config.base_global_planner.c_str(), ex.what());
        planner_ = old_planner;
        config.base_global_planner = last_config_.base_global_planner;
      }
    }

    if(config.base_local_planner != last_config_.base_local_planner){
     
      boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
      //创建局部路径规划
      try {
     
        tc_ = blp_loader_.createInstance(config.base_local_planner);
        // 在新规划开始之前清除旧的
        planner_plan_->clear();
        latest_plan_->clear();
        controller_plan_->clear();
        resetState();
        tc_->initialize(blp_loader_.getName(config.base_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", config.base_local_planner.c_str(), ex.what());
        tc_ = old_planner;
        config.base_local_planner = last_config_.base_local_planner;
      }
    }

    make_plan_clear_costmap_ = config.make_plan_clear_costmap;
    make_plan_add_unreachable_goal_ = config.make_plan_add_unreachable_goal;

    last_config_ = config;
  }