Ask Your Question
0

How to switch the planner of move_base during the navigation

asked 2017-07-10 19:54:14 -0600

scopus gravatar image

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)

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2017-07-11 03:43:56 -0600

Akif gravatar image

updated 2017-07-11 07:33:04 -0600

Instead of messing with move_base source, as mentioned in the question you have linked, you can write a planner which will get goal from move_base and pass to your desired planner in a conditional manner.

In move_base side, you will have one active planner which is your parent planner. It will never change. But in your planner you will initialize all planners you might use and pass the goal obtained from move_base to desired planner. Even you can change active planner with dynamic reconfigure.

Additional explanation:

You will not change anything in move_base source. Just write a new global planner plugin ( http://wiki.ros.org/navigation/Tutori... ), then use this planner as your global planner. In initialize method of your new plugin, initialize other global plugins you desire (like navfn or global_planner) then in makePlan method of your new plugin, just pass start, goal and plan parameters directly to makePlan method of the planner you want. And of course, return methods accordingly.

Hope this explains better.

edit flag offensive delete link more

Comments

Actually, I can't understand what the linked question said. I don't understand what " supervisory planner" is. I have successfully implemented a planner "LinearGlobalPlanner" and tested it in movebase.

scopus gravatar imagescopus ( 2017-07-11 06:17:31 -0600 )edit

Thank you for your advice. But I still can't get your idea. Do you mean I can add a memeber variable as " pluginlib::ClassLoader<nav_core::baselocalplanner> blp_loader_" into MoveBase class to initialize my planner? Would you like to describe more clearly?

scopus gravatar imagescopus ( 2017-07-11 06:21:14 -0600 )edit

Thank you! I think I get your idea. Do you mean: write a new planner plugin which contains different path planner algorithm? Because the makeplan method only has start, goal and plan parameters, the body of this method can contains more than one plan algorithm in the meantime

scopus gravatar imagescopus ( 2017-07-11 20:00:09 -0600 )edit

I think you are correct. Thank you! I mark your post as correct answer..

scopus gravatar imagescopus ( 2017-07-11 20:17:43 -0600 )edit

You do not need to reimplement navfn or global_planner in your custom plugin. For example, you can include #include <navfn/navfn_ros.h> and declare navfn as navfn::NavfnROS navfn_planner; in your planner. Just call navfn_planner.makePlan(start, goal, plan); in your plugin's makePlan method.

Akif gravatar imageAkif ( 2017-07-12 00:55:17 -0600 )edit

Thank you! I am going to implement and test it!

scopus gravatar imagescopus ( 2017-07-12 06:05:39 -0600 )edit

Thank you! I succeded to implement the supervisory global planner. But when I implement the supervisory local planner. I encountered a problem which I have asked here. Please do me a favor. Thanks.

scopus gravatar imagescopus ( 2017-07-13 21:25:13 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-07-10 19:54:14 -0600

Seen: 1,233 times

Last updated: Jul 11 '17