$search

global_planner_switcher.cpp

Go to the documentation of this file.

#include <nav_core/base_global_planner.h>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/String.h>

#include <sbpl_cart_planner/sbpl_cart_planner.h>
#include <navfn/navfn_ros.h>

namespace global_planner_switcher {

class GlobalPlannerSwitcher : public nav_core::BaseGlobalPlanner
{
public:
  GlobalPlannerSwitcher()
  {
      sbpl_planner_= new SBPLCartPlanner();
      navfn_planner_ = new navfn::NavfnROS();

      comp_ctor("global_planner_switcher");
  }


  GlobalPlannerSwitcher(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
  {
      sbpl_planner_ = new SBPLCartPlanner("SBPLCartPlanner", costmap_ros);
      navfn_planner_ = new navfn::NavfnROS("navfn/NavfnROS", costmap_ros);

      comp_ctor(name);
  }

  void initialize(std::string name,
                          costmap_2d::Costmap2DROS* costmap_ros)
  {
      sbpl_planner_->initialize("SBPLCartPlanner", costmap_ros);
      navfn_planner_->initialize("navfn/NavfnROS", costmap_ros);
  }

  virtual bool makePlan(const geometry_msgs::PoseStamped& start,
                        const geometry_msgs::PoseStamped& goal,
                        std::vector<geometry_msgs::PoseStamped>& plan)
  {
      if (sbpl_active_)
          return sbpl_planner_->makePlan(start, goal, plan);
      else
          return navfn_planner_->makePlan(start, goal, plan);
  }

  ~GlobalPlannerSwitcher()
  {
      delete sbpl_planner_;
      delete navfn_planner_;
  }

private:
  SBPLCartPlanner *sbpl_planner_;
  navfn::NavfnROS *navfn_planner_;

  ros::NodeHandle nh_;

  void setPlannerCB(const std_msgs::String &msg)
  {
      sbpl_active_ = (msg.data == "sbpl");
      ROS_INFO("Switching global planner. SBPL active: %d", sbpl_active_);

      std_msgs::String status;
      if (sbpl_active_) {
          status.data = "sbpl";
      }
      else {
          status.data = "navfn";
      }

      active_planner_pub_.publish(status);
  }

  void comp_ctor(const std::string &name)
  {
      sbpl_active_ = false;

      nh_ = ros::NodeHandle("move_base_node/global_planner_switcher");
      active_planner_sub_ = nh_.subscribe("set_active_planner", 1, &GlobalPlannerSwitcher::setPlannerCB, this);
      active_planner_pub_ = nh_.advertise<std_msgs::String>("active_planner", 5, true);

      std_msgs::String status;
      status.data = "navfn";
      active_planner_pub_.publish(status);
  }

  bool sbpl_active_;

  ros::Subscriber active_planner_sub_;
  ros::Publisher active_planner_pub_;
};

PLUGINLIB_DECLARE_CLASS(global_planner_switcher, GlobalPlannerSwitcher, global_planner_switcher::GlobalPlannerSwitcher, nav_core::BaseGlobalPlanner);

} // namespace

---

global_planner_switcher
Author(s): Kevin Watts
autogenerated on Fri Mar 1 14:56:13 2013