stuck_robot_monitor.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00003 #include <maiss_nav/ObservedState.h>
00004 #include <move_base_msgs/MoveBaseActionResult.h>
00005 #include <move_base_msgs/MoveBaseActionGoal.h>
00006 #include <move_base_msgs/MoveBaseAction.h>
00007 #include <actionlib/client/simple_action_client.h>
00008 
00009 #include <math.h>
00010 #include <stdlib.h>
00011 
00012 
00013 
00014 using namespace ros;
00015 using namespace move_base_msgs;
00016 
00017 
00018 
00019 typedef actionlib::SimpleActionClient<MoveBaseAction> MoveBaseClient;
00020 
00021 
00022 
00023 class StuckRobotMonitor
00024 {
00025   private:
00027     void resultReceivedCallback (const MoveBaseActionResult& msg);
00028     void currentGoalCallback (const MoveBaseActionGoal& msg);
00029     
00030     NodeHandle n_;
00031     
00032     
00033     
00034     Subscriber result_sub_;
00035     Subscriber goal_sub_;
00036     Publisher goal_pub_;
00037     boost::shared_ptr<MoveBaseClient> mb_client_;
00038     
00039   public:
00040     StuckRobotMonitor() :
00041       n_(),
00042       result_sub_ (n_.subscribe ("move_base/result", 1, &StuckRobotMonitor::resultReceivedCallback, this)),
00043       goal_sub_ (n_.subscribe ("move_base/goal", 1, &StuckRobotMonitor::currentGoalCallback, this)),
00044       mb_client_ (new MoveBaseClient ("move_base", true)) {
00045       mb_client_->waitForServer();
00046     }
00047     
00048     MoveBaseGoal currentGoal_;
00049     
00050     ~StuckRobotMonitor() {}
00051 };
00052 
00053 
00054 
00058 void StuckRobotMonitor::resultReceivedCallback (const MoveBaseActionResult& msg)
00059 {
00060   if (msg.status.status == 4) { // robot is stuck, re-send the last goal to force it to move (if possible)
00061     ROS_ERROR ("Resending goal");
00062     
00063     mb_client_->sendGoal (currentGoal_);
00064   }
00065 }
00066 
00067 
00068 
00069 void StuckRobotMonitor::currentGoalCallback (const MoveBaseActionGoal& msg)
00070 {
00071   currentGoal_ = msg.goal;
00072 }
00073 
00074 
00075 
00077 int main (int argc, char** argv)
00078 {
00079   init (argc, argv, "stuck_robot_monitor");
00080   
00081   StuckRobotMonitor srm;
00082   
00083   spin();
00084   
00085   return 0;
00086 }


nav
Author(s): Raul Perula-Martinez , Jose Carlos Castillo Montoya
autogenerated on Thu Apr 23 2015 22:20:03