Go to the documentation of this file.00001
00009
00010
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_dabo_gui/qnode_dabo_coop.hpp"
00019
00020
00021
00022
00023
00024 namespace tibi_dabo_gui {
00025
00026
00027
00028
00029
00030 QNode::QNode(int argc, char** argv ) :
00031 init_argc(argc),
00032 init_argv(argv),
00033 gui_action("gui",true),
00034 gui_action_aserver_(n, "gui_action_server")
00035 {}
00036
00037 QNode::~QNode() {
00038 if(ros::isStarted()) {
00039 ros::shutdown();
00040 ros::waitForShutdown();
00041 }
00042 wait();
00043 }
00044
00045 bool QNode::initialize()
00046 {
00047 if ( ! ros::master::check() ) {
00048 return false;
00049 }
00050 ros::start();
00051
00052
00053
00054 get_locations_client = n.serviceClient<iri_goal_database::get_locations>("get_locations");
00055
00056
00057 this->gui_active=false;
00058
00059
00060 gui_action_aserver_.registerStartCallback(boost::bind(&QNode::gui_actionStartCallback, this, _1));
00061 gui_action_aserver_.registerStopCallback(boost::bind(&QNode::gui_actionStopCallback, this));
00062 gui_action_aserver_.registerIsFinishedCallback(boost::bind(&QNode::gui_actionIsFinishedCallback, this));
00063 gui_action_aserver_.registerHasSucceedCallback(boost::bind(&QNode::gui_actionHasSucceedCallback, this));
00064 gui_action_aserver_.registerGetResultCallback(boost::bind(&QNode::gui_actionGetResultCallback, this, _1));
00065 gui_action_aserver_.registerGetFeedbackCallback(boost::bind(&QNode::gui_actionGetFeedbackCallback, this, _1));
00066 gui_action_aserver_.start();
00067 this->new_goal_cmd=false;
00068 this->new_sentence_cmd=false;
00069 this->gui_cmd=gui_none;
00070
00071 start();
00072 return true;
00073 }
00074
00075 void QNode::run() {
00076 ros::Rate loop_rate(100);
00077 int count = 0;
00078 while ( ros::ok() )
00079 {
00080 ros::spinOnce();
00081 loop_rate.sleep();
00082 ++count;
00083 }
00084 std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
00085 Q_EMIT rosShutdown();
00086 }
00087
00088 void QNode::exit()
00089 {
00090 mutex.lock();
00091 if(!this->gui_active)
00092 {
00093 this->gui_active=true;
00094 this->gui_goal.cmd_id=0;
00095 if(!guiMakeActionRequest())
00096 ROS_ERROR("QNode::exit: guiMakeActionRequest failed!");
00097 }
00098 else
00099 ROS_INFO("QNode::exit: guiAction active, goal not sent!");
00100 mutex.unlock();
00101 }
00102
00103 bool QNode::getLocations(std::vector<std::string> & locations)
00104 {
00105
00106 bool ok=true;
00107 if(get_locations_client.call(this->get_locations_srv))
00108 {
00109 for(unsigned int i=0; i<this->get_locations_srv.response.locations.size(); i++)
00110 {
00111 locations.push_back(this->get_locations_srv.response.locations[i]);
00112 }
00113 }
00114 else
00115 {
00116 ok=false;
00117 ROS_ERROR("QNode::getLocations: unable to call get_locations service!");
00118 }
00119 return ok;
00120 }
00121
00122 bool QNode::getGoal(std::string & goal)
00123 {
00124 bool ok=true;
00125 if(this->new_goal_cmd)
00126 {
00127 goal=this->gui_location_id;
00128 this->new_goal_cmd=false;
00129 }
00130 else
00131 ok=false;
00132 return ok;
00133 }
00134
00135 bool QNode::getSentence(double & sentence)
00136 {
00137 bool ok=true;
00138 if(this->new_sentence_cmd)
00139 {
00140 sentence=this->sentence_id;
00141 this->new_sentence_cmd=false;
00142 }
00143 else
00144 ok=false;
00145 return ok;
00146 }
00147
00148
00149 void QNode::gui_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
00150 {
00151 ROS_INFO("QNode::gui_actionStartCallback");
00152 mutex.lock();
00153
00154 this->gui_cmd=(gui_cmds)goal->cmd_id;
00155 switch(this->gui_cmd)
00156 {
00157 case gui_set_goal:
00158 if(goal->string_params.size()==1)
00159 {
00160 this->gui_location_id=goal->string_params[0];
00161 this->new_goal_cmd=true;
00162 }
00163 else
00164 this->new_goal_cmd=false;
00165 break;
00166 case gui_set_sentence:
00167 if(goal->num_params.size()==1)
00168 {
00169 this->sentence_id = goal->num_params[0];
00170 this->new_sentence_cmd=true;
00171 }
00172 else
00173 this->new_sentence_cmd=false;
00174 break;
00175 default: this->new_goal_cmd=false;
00176 this->new_sentence_cmd=false;
00177 break;
00178 }
00179 mutex.unlock();
00180 }
00181
00182 void QNode::gui_actionStopCallback(void)
00183 {
00184 mutex.lock();
00185
00186
00187 mutex.unlock();
00188 }
00189
00190 bool QNode::gui_actionIsFinishedCallback(void)
00191 {
00192 bool ret = false;
00193
00194 mutex.lock();
00195
00196 ret = true;
00197 mutex.unlock();
00198
00199 return ret;
00200 }
00201
00202 bool QNode::gui_actionHasSucceedCallback(void)
00203 {
00204 bool ret = false;
00205
00206 mutex.lock();
00207
00208 ret=true;
00209
00210 mutex.unlock();
00211
00212 return ret;
00213 }
00214
00215 void QNode::gui_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result)
00216 {
00217 mutex.lock();
00218
00219
00220 mutex.unlock();
00221 }
00222
00223 void QNode::gui_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback)
00224 {
00225 mutex.lock();
00226
00227
00228 mutex.unlock();
00229 }
00230
00231
00232 void QNode::guiDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::guiResultConstPtr& result)
00233 {
00234 mutex.lock();
00235 if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
00236 ROS_INFO("QNode::guiDone: Goal Achieved!");
00237 else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
00238 ROS_INFO("QNode::guiDone: Goal aborted!");
00239 else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
00240 ROS_INFO("QNode::guiDone: Goal preempted!");
00241 else
00242 ROS_INFO("QNode::guiDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
00243 this->gui_active=false;
00244 mutex.unlock();
00245 }
00246
00247 void QNode::guiActive()
00248 {
00249 mutex.lock();
00250 ROS_INFO("QNode::guiActive: Goal just went active!");
00251 this->gui_active=true;
00252 mutex.unlock();
00253 }
00254
00255 void QNode::guiFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback)
00256 {
00257 mutex.lock();
00258
00259 bool feedback_is_ok = true;
00260 if( !feedback_is_ok )
00261 gui_action.cancelGoal();
00262 mutex.unlock();
00263 }
00264
00265 bool QNode::guiMakeActionRequest()
00266 {
00267 ROS_INFO("QNode::guiMakeActionRequest: Starting New Request!");
00268 gui_action.waitForServer();
00269 ROS_INFO("QNode::guiMakeActionRequest: Server is available!");
00270 gui_action.sendGoal(this->gui_goal,
00271 boost::bind(&QNode::guiDone, this, _1, _2),
00272 boost::bind(&QNode::guiActive, this),
00273 boost::bind(&QNode::guiFeedback, this, _1));
00274 ROS_INFO("QNode::guiMakeActionRequest: Goal Sent. Wait for Result!");
00275 return true;
00276 }
00277
00278 }