0
$\begingroup$

Rosanswers logo

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 funtion of a rosservice. There is a script to call the service and switch the planner. The default global planner is "linear_global_planner/LinearGlobalPlanner" which has been implemented and verified by ourselves. The default local planner is "ftc_local_planner/FTCPlanner". When calling the service, the expected results is the global planner is switched to "asr_navfn/NavfnROS", and the local planner is switchced to "dwa_local_planner/DWAPlannerROS". However, in the experiments, the following error is output in the terimnal:

[FATAL] [1499686752.366971926, 1097.860000000]: Failed to create the navfn/NavfnROS planner, are you sure it is properly registered and that the containing library is built? Exception: According to the loaded plugin descriptions the class navfn/NavfnROS with base class type nav_core::BaseLocalPlanner does not exist. Declared types are  base_local_planner/TrajectoryPlannerROS dwa_local_planner/DWAPlannerROS ftc_local_planner/FTCPlanner
move_base: /usr/include/boost/thread/pthread/recursive_mutex.hpp:101: boost::recursive_mutex::~recursive_mutex(): Assertion `!pthread_mutex_destroy(&m)' failed.

Can anyone give me some valuable advices?

Thank you very much!


Originally posted by scopus on ROS Answers with karma: 279 on 2017-07-10

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS), 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.


Originally posted by Akif with karma: 3561 on 2017-07-11

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by scopus on 2017-07-11:
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.

Comment by scopus on 2017-07-11:
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?

Comment by scopus on 2017-07-11:
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

Comment by scopus on 2017-07-11:
I think you are correct. Thank you! I mark your post as correct answer..

Comment by Akif on 2017-07-12:
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.

Comment by scopus on 2017-07-12:
Thank you! I am going to implement and test it!

Comment by scopus on 2017-07-13:
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.

Comment by troyelex on 2021-08-20:
is it worked? were you able to use your implemented planner plugin?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.