How to switch the planner of move_base during the navigation

edit

Hi, all, We want to implement a function in move_base to switch the planners as this question mentioned. To do this ,I have studied the source code of move base, specifically the constructor of movebase and the reconfigureCB member method, because the settings of planner is done in these two member methods. By imitating the constructor of movebase, I added a member method into the movebase as follows:

 bool MoveBase::changepl( asr_navfn::PlannerSwitcher::Request &req, asr_navfn::PlannerSwitcher::Response &res ){if(req.changePlanner.compare("switch on") == 0)
  {
      //Need to clear?
      ROS_INFO("Hello, switch on, going to be change planner");
      as_->shutdown();
      planner_plan_->clear();
      latest_plan_->clear();
      controller_plan_->clear();


      std::string global_planner, local_planner;
      if(req.globalName.empty())
          global_planner= "asr_navfn/NavfnROS";
      else
          global_planner= req.globalName;

      if(req.localName.empty())
          local_planner= "dwa_local_planner/DWAPlannerROS";
      else
          local_planner= req.globalName;

       planner_costmap_ros_->pause();


      try {
          //bgp_loader_.loadLibraryForClass();
           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);
         }
        controller_costmap_ros_->pause();

         //change a local planner
         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_);
           tc_->setGlobalCostmap(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", local_planner.c_str(), ex.what());
           exit(1);
     }
        planner_costmap_ros_->start();
       controller_costmap_ros_->start();

       if(shutdown_costmaps_){
         ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
         planner_costmap_ros_->stop();
         controller_costmap_ros_->stop();
       }
       //initially, we'll need to make a plan
       state_ = PLANNING;

        //we'll start executing recovery behaviors at the beginning of our list
       recovery_index_ = 0;
       as_->start();

      res.changeResult=true;
      return true;


  }
  else if(req.changePlanner.compare("switch off") == 0)
  {
      ROS_INFO("Hello, switch off, going to be change planner");
      as_->shutdown();
      planner_plan_->clear();
      latest_plan_->clear();
      controller_plan_->clear();


      std::string global_planner, local_planner;
      global_planner= "linear_global_planner/LinearGlobalPlanner";
      local_planner= "ftc_local_planner/FTCPlanner";
      planner_costmap_ros_->pause();


      try {
          //bgp_loader_.loadLibraryForClass();
           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);
         }

        controller_costmap_ros_->pause();

         //create a local planner
         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_);
           tc_->setGlobalCostmap(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", local_planner.c_str(), ex.what());
           exit(1);
     }
        planner_costmap_ros_->start();
       controller_costmap_ros_->start();

       if(shutdown_costmaps_){
         ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
         planner_costmap_ros_->stop();
         controller_costmap_ros_->stop();
       }
       //initially, we'll need to make a plan
       state_ = PLANNING;

        //we'll start executing recovery behaviors at the beginning of our list
       recovery_index_ = 0;
       as_->start();

      res.changeResult=true;
      return true;

  }
  else
  {
      ROS_INFO("Hello, return false, didn't change planner");
      res.changeResult= false;
      return false;
  }}

The above memeber method is a callback ... (more)