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/dabo_coop_gui/qnode.hpp"
00019
00020
00021
00022
00023
00024 namespace dabo_coop_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_gui_cmd=false;
00068 this->gui_cmd=gui_none;
00069
00070 start();
00071 return true;
00072 }
00073
00074 void QNode::run() {
00075 ros::Rate loop_rate(100);
00076 int count = 0;
00077 while ( ros::ok() )
00078 {
00079 ros::spinOnce();
00080 loop_rate.sleep();
00081 ++count;
00082 }
00083 std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
00084 Q_EMIT rosShutdown();
00085 }
00086
00087 void QNode::exit()
00088 {
00089 mutex.lock();
00090 if(!this->gui_active)
00091 {
00092 this->gui_active=true;
00093 this->gui_goal.cmd_id=0;
00094 if(!guiMakeActionRequest())
00095 ROS_ERROR("QNode::exit: guiMakeActionRequest failed!");
00096 }
00097 else
00098 ROS_INFO("QNode::exit: guiAction active, goal not sent!");
00099 mutex.unlock();
00100 }
00101
00102 bool QNode::getLocations(std::vector<std::string> & locations)
00103 {
00104
00105 bool ok=true;
00106 if(get_locations_client.call(this->get_locations_srv))
00107 {
00108 for(unsigned int i=0; i<this->get_locations_srv.response.locations.size(); i++)
00109 {
00110 locations.push_back(this->get_locations_srv.response.locations[i]);
00111 }
00112 }
00113 else
00114 {
00115 ok=false;
00116 ROS_ERROR("QNode::getLocations: unable to call get_locations service!");
00117 }
00118 return ok;
00119 }
00120
00121 bool QNode::getGoal(std::string & goal)
00122 {
00123 bool ok=true;
00124 if(this->new_gui_cmd)
00125 {
00126 goal=this->gui_location_id;
00127 this->new_gui_cmd=false;
00128 }
00129 else
00130 ok=false;
00131 return ok;
00132 }
00133
00134
00135 void QNode::gui_actionStartCallback(const tibi_dabo_msgs::guiGoalConstPtr& goal)
00136 {
00137 ROS_INFO("QNode::gui_actionStartCallback");
00138 mutex.lock();
00139
00140 this->gui_cmd=(gui_cmds)goal->cmd_id;
00141 switch(this->gui_cmd)
00142 {
00143 case gui_set_goal:
00144 if(goal->string_params.size()==1)
00145 {
00146 this->gui_location_id=goal->string_params[0];
00147 this->new_gui_cmd=true;
00148 }
00149 else
00150 this->new_gui_cmd=false;
00151 break;
00152 default: this->new_gui_cmd=false;
00153 break;
00154 }
00155 mutex.unlock();
00156 }
00157
00158 void QNode::gui_actionStopCallback(void)
00159 {
00160 mutex.lock();
00161
00162
00163 mutex.unlock();
00164 }
00165
00166 bool QNode::gui_actionIsFinishedCallback(void)
00167 {
00168 bool ret = false;
00169
00170 mutex.lock();
00171
00172 ret = true;
00173 mutex.unlock();
00174
00175 return ret;
00176 }
00177
00178 bool QNode::gui_actionHasSucceedCallback(void)
00179 {
00180 bool ret = false;
00181
00182 mutex.lock();
00183
00184 ret=true;
00185
00186 mutex.unlock();
00187
00188 return ret;
00189 }
00190
00191 void QNode::gui_actionGetResultCallback(tibi_dabo_msgs::guiResultPtr& result)
00192 {
00193 mutex.lock();
00194
00195
00196 mutex.unlock();
00197 }
00198
00199 void QNode::gui_actionGetFeedbackCallback(tibi_dabo_msgs::guiFeedbackPtr& feedback)
00200 {
00201 mutex.lock();
00202
00203
00204 mutex.unlock();
00205 }
00206
00207
00208 void QNode::guiDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::guiResultConstPtr& result)
00209 {
00210 mutex.lock();
00211 if(state.state_==actionlib::SimpleClientGoalState::SUCCEEDED)
00212 ROS_INFO("QNode::guiDone: Goal Achieved!");
00213 else if(state.state_==actionlib::SimpleClientGoalState::ABORTED)
00214 ROS_INFO("QNode::guiDone: Goal aborted!");
00215 else if(state.state_==actionlib::SimpleClientGoalState::PREEMPTED)
00216 ROS_INFO("QNode::guiDone: Goal preempted!");
00217 else
00218 ROS_INFO("QNode::guiDone: Other termination state: PENDING, ACTIVE, RECALLED, REJECTED, LOST");
00219 this->gui_active=false;
00220 mutex.unlock();
00221 }
00222
00223 void QNode::guiActive()
00224 {
00225 mutex.lock();
00226 ROS_INFO("QNode::guiActive: Goal just went active!");
00227 this->gui_active=true;
00228 mutex.unlock();
00229 }
00230
00231 void QNode::guiFeedback(const tibi_dabo_msgs::guiFeedbackConstPtr& feedback)
00232 {
00233 mutex.lock();
00234
00235 bool feedback_is_ok = true;
00236 if( !feedback_is_ok )
00237 gui_action.cancelGoal();
00238 mutex.unlock();
00239 }
00240
00241 bool QNode::guiMakeActionRequest()
00242 {
00243 ROS_INFO("QNode::guiMakeActionRequest: Starting New Request!");
00244 gui_action.waitForServer();
00245 ROS_INFO("QNode::guiMakeActionRequest: Server is available!");
00246 gui_action.sendGoal(this->gui_goal,
00247 boost::bind(&QNode::guiDone, this, _1, _2),
00248 boost::bind(&QNode::guiActive, this),
00249 boost::bind(&QNode::guiFeedback, this, _1));
00250 ROS_INFO("QNode::guiMakeActionRequest: Goal Sent. Wait for Result!");
00251 return true;
00252 }
00253
00254 }