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_coop_gui/qnode.hpp"
00019
00020
00021
00022
00023
00024 namespace tibi_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 {}
00035
00036 QNode::~QNode() {
00037 if(ros::isStarted()) {
00038 ros::shutdown();
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();
00050 ros::NodeHandle n;
00051
00052
00053 get_locations_client = n.serviceClient<iri_goal_database::get_locations>("get_locations");
00054
00055
00056
00057
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();
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
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
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
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 }