qnode.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Includes
00011 *****************************************************************************/
00012 
00013 #include <ros/ros.h>
00014 #include <ros/network.h>
00015 #include <string>
00016 #include <std_msgs/String.h>
00017 #include <sstream>
00018 #include "../include/tibi_coop_gui/qnode.hpp"
00019 
00020 /*****************************************************************************
00021 ** Namespaces
00022 *****************************************************************************/
00023 
00024 namespace tibi_coop_gui {
00025 
00026 /*****************************************************************************
00027 ** Implementation
00028 *****************************************************************************/
00029 
00030 QNode::QNode(int argc, char** argv ) :
00031   init_argc(argc),
00032   init_argv(argv),
00033   gui_action("gui",true)
00034   {}
00035 
00036 QNode::~QNode() {
00037   if(ros::isStarted()) {
00038     ros::shutdown(); // explicitly needed since we use ros::start();
00039     ros::waitForShutdown();
00040   }
00041   wait();
00042 }
00043 
00044 bool QNode::initialize()
00045 {
00046   if ( ! ros::master::check() ) {
00047       return false;
00048   }
00049   ros::start(); // explicitly needed since our nodehandle is going out of scope.
00050   ros::NodeHandle n;
00051 
00052   // [service clients]
00053   get_locations_client = n.serviceClient<iri_goal_database::get_locations>("get_locations");
00054 
00055   //[action clients]
00056       // this->gui_goal.FIELD=X
00057       // guiMakeActionRequest();
00058   this->gui_active=false;
00059 
00060   start();
00061   return true;
00062 }
00063 
00064 void QNode::run() {
00065   ros::Rate loop_rate(100);
00066   int count = 0;
00067   while ( ros::ok() )
00068   {
00069     ros::spinOnce();
00070     loop_rate.sleep();
00071     ++count;
00072   }
00073   std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
00074   Q_EMIT rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
00075 }
00076 
00077 void QNode::askGoal(std::string goal)
00078 {
00079   mutex.lock();
00080   if(!this->gui_active)
00081   {
00082     this->gui_active=true;
00083     this->gui_goal.cmd_id=0;
00084     this->gui_goal.string_params.resize(1);
00085     this->gui_goal.string_params[0]=goal;
00086     if(!guiMakeActionRequest())
00087       ROS_ERROR("QNode::askGoal: guiMakeActionRequest failed!");
00088   }
00089   else
00090     ROS_INFO("QNode::askGoal: guiAction active, goal not sent!");
00091   mutex.unlock();
00092 }
00093 
00094 bool QNode::getLocations(std::vector<std::string> & locations)
00095 {
00096   mutex.lock();
00097   //get locations from database service, fill locations vector
00098   bool ok=true;
00099   if(get_locations_client.call(this->get_locations_srv))
00100   {
00101     for(unsigned int i=0; i<this->get_locations_srv.response.locations.size(); i++)
00102     {
00103       locations.push_back(this->get_locations_srv.response.locations[i]);
00104     }
00105   }
00106   else
00107   {
00108     ok=false;
00109     ROS_ERROR("QNode::getLocations: unable to call get_locations service!");
00110   }
00111   mutex.unlock();
00112   return ok;
00113 }
00114 
00115 bool QNode::guiActionActive()
00116 {
00117   return this->gui_active;
00118 }
00119 
00120 //gui action client functions
00121 void QNode::guiDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::guiResultConstPtr& result)
00122 {
00123   mutex.lock();
00124   if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
00125     ROS_INFO("QNode::guiDone: Goal Achieved!");
00126   else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
00127     ROS_INFO("QNode::guiDone: Goal aborted!");
00128   else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
00129     ROS_INFO("QNode::guiDone: Goal preempted!");
00130   else
00131     ROS_INFO("QNode::guiDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
00132   this->gui_active=false;
00133   mutex.unlock();
00134 }
00135 
00136 void QNode::guiActive()
00137 {
00138   mutex.lock();
00139   ROS_INFO("QNode::guiActive: Goal just went active!"); 
00140   this->gui_active=true;
00141   mutex.unlock();
00142 }
00143 
00144 void QNode::guiFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback)
00145 {
00146   mutex.lock();
00147   //ROS_INFO("QNode::guiFeedback: feedback received!");
00148   bool feedback_is_ok = true;
00149   if( !feedback_is_ok )
00150     gui_action.cancelGoal();
00151   mutex.unlock();
00152 }
00153 
00154 bool QNode::guiMakeActionRequest() 
00155 {
00156    ROS_INFO("QNode::guiMakeActionRequest: Starting New Request!"); 
00157    gui_action.waitForServer();
00158    ROS_INFO("QNode::guiMakeActionRequest: Server is available!"); 
00159    gui_action.sendGoal(this->gui_goal, 
00160                boost::bind(&QNode::guiDone,     this, _1, _2), 
00161                boost::bind(&QNode::guiActive,   this), 
00162                boost::bind(&QNode::guiFeedback, this, _1)); 
00163    ROS_INFO("QNode::guiMakeActionRequest: Goal Sent. Wait for Result!"); 
00164    return true;
00165 }
00166 
00167 }  // namespace tibi_coop_gui


tibi_coop_gui
Author(s): fherrero
autogenerated on Fri Dec 6 2013 23:44:26