move_base_interruptable_server.cpp
Go to the documentation of this file.
00001 #include <boost/thread/thread.hpp>
00002 #include <bwi_interruptable_action_server/interruptable_action_server.h>
00003 #include <move_base_msgs/MoveBaseAction.h>
00004 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00005 #include <nav_msgs/GetPlan.h>
00006 #include <ros/ros.h>
00007 #include <std_srvs/Empty.h>
00008 
00009 using namespace bwi_interruptable_action_server;
00010 
00011 boost::shared_ptr<ros::ServiceClient> clear_costmap_service_;
00012 boost::shared_ptr<ros::ServiceClient> make_plan_service_;
00013 boost::shared_ptr<ros::Subscriber> location_subscriber_;
00014 bool first_location_available_ = false;
00015 geometry_msgs::PoseStamped current_location_;
00016 
00017 void locationHandler(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& pose) {
00018   current_location_.header = pose->header;
00019   current_location_.pose = pose->pose.pose;
00020   first_location_available_ = true;
00021 }
00022 
00023 void newGoalCallback(const InterruptableActionServer<move_base_msgs::MoveBaseAction>::GoalConstPtr& new_goal) {
00024   if (first_location_available_) {
00025     // TODO also check that our current location estimates are not too far off.
00026     nav_msgs::GetPlan srv;
00027     srv.request.start = current_location_;
00028     srv.request.goal = new_goal->target_pose;
00029     srv.request.tolerance = 0.0f;
00030     bool plan_found = false;
00031     if (make_plan_service_->call(srv)) {
00032       plan_found = srv.response.plan.poses.size() != 0;
00033     }
00034     if (!plan_found) {
00035       ROS_INFO_STREAM("New goal received, but unable to find plan to goal. Clearing costmaps before sending goal to nav stack.");
00036       std_srvs::Empty srv;
00037       if (!clear_costmap_service_->call(srv)) {
00038           // this sometimes crashes move_base
00039         ROS_ERROR_STREAM("Unable to clear costmaps!");
00040       } else {
00041         // Sleep for three seconds to allow costmaps to be cleared and reset properly.
00042         boost::this_thread::sleep(boost::posix_time::milliseconds(3000));
00043         ROS_INFO_STREAM("Costmaps cleared!");
00044       }
00045     }
00046   }
00047 }
00048 
00049 int main(int argc, char *argv[]) {
00050   ros::init(argc, argv, "move_base_interruptable_server");
00051   ros::NodeHandle nh;
00052 
00053   ROS_INFO_STREAM("move_base_interruptable : Waiting for navigation services...");
00054   clear_costmap_service_.reset(new ros::ServiceClient);
00055   *clear_costmap_service_ = nh.serviceClient<std_srvs::Empty>("move_base/clear_costmaps");
00056   clear_costmap_service_->waitForExistence();
00057 
00058   make_plan_service_.reset(new ros::ServiceClient);
00059   *make_plan_service_ = nh.serviceClient<nav_msgs::GetPlan>("move_base/NavfnROS/make_plan");
00060   make_plan_service_->waitForExistence();
00061 
00062   ROS_INFO_STREAM("move_base_interruptable :   Navigation services found!");
00063 
00064   location_subscriber_.reset(new ros::Subscriber);
00065   *location_subscriber_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 1, &locationHandler);
00066 
00067   InterruptableActionServer<move_base_msgs::MoveBaseAction> as(nh, "move_base", 1, &newGoalCallback);
00068   as.spin(); // internally calls ros::spinOnce where locationHandler will also get called.
00069   
00070   return 0;
00071 }


segbot_navigation
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 21:37:29