Go to the documentation of this file.00001 #include "voice_recognition_client_alg_node.h"
00002
00003 VoiceRecognitionClientAlgNode::VoiceRecognitionClientAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<VoiceRecognitionClientAlgorithm>(),
00005 hri_client_("hri", true),
00006 voice_recognition_client_("voice_recognition", true)
00007 {
00008
00009
00010
00011
00012
00013 this->code_publisher_ = this->public_node_handle_.advertise<iri_perception_msgs::voiceRecognitionAnswer>("answerCode", 100);
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 this->voice_recognition_action_in_progress = false;
00025 this->voice_recognition_done = false;
00026 this->hri_action_in_progress = false;
00027 this->lastStatus_ = 0;
00028 this->code_ = 0;
00029 this->lastCode_ = 0;
00030
00031 hri_goal_.sequence_file = std::vector<std::string>(NUM_CLIENTS, std::string(""));
00032 hri_goal_.num_repetitions = std::vector<int>(NUM_CLIENTS, 0);
00033 }
00034
00035 VoiceRecognitionClientAlgNode::~VoiceRecognitionClientAlgNode(void)
00036 {
00037
00038 }
00039
00040 void VoiceRecognitionClientAlgNode::mainNodeThread(void)
00041 {
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 bool run = true;
00052 if(run && !this->voice_recognition_action_in_progress && !this->hri_action_in_progress && !this->voice_recognition_done)
00053 {
00054 this->voice_recognition_action_in_progress = true;
00055 voice_recognitionMakeActionRequest();
00056 this->voice_recognition_done = true;
00057 }
00058
00059 if(this->voice_recognition_done && !this->hri_action_in_progress)
00060 {
00061 if(this->answer_.size()!=0)
00062 {
00063
00064 this->hri_action_in_progress = true;
00065
00066 hriMakeActionRequest();
00067 this->hri_action_in_progress = false;
00068
00069 }
00070 this->voice_recognition_done = false;
00071 }
00072
00073
00074
00075 if(this->code_ != this->lastCode_)
00076 {
00077
00078
00079
00080
00081 this->voiceRecognitionAnswer_msg_.code = this->code_;
00082 this->voiceRecognitionAnswer_msg_.answer = this->answer_;
00083 this->code_publisher_.publish(this->voiceRecognitionAnswer_msg_);
00084 this->lastCode_ = this->code_;
00085
00086 if(this->answer_.size()!=0)
00087 {
00088 if(this->code_<100)
00089 {
00090 sleep(3);
00091 }
00092 else
00093 {
00094 sleep(4);
00095 }
00096 }
00097
00098 this->answer_.clear();
00099 }
00100 }
00101
00102
00103
00104
00105
00106
00107 void VoiceRecognitionClientAlgNode::hriDone(const actionlib::SimpleClientGoalState& state, const tibi_dabo_msgs::sequenceResultConstPtr& result)
00108 {
00109 if( state.toString().compare("SUCCEEDED") == 0 )
00110 ROS_INFO("VoiceRecognitionClientAlgNode::hriDone: Goal Achieved!");
00111 else
00112 ROS_INFO("VoiceRecognitionClientAlgNode::hriDone: %s", state.toString().c_str());
00113
00114
00115 this->hri_action_in_progress = false;
00116 }
00117
00118 void VoiceRecognitionClientAlgNode::hriActive()
00119 {
00120
00121 }
00122
00123 void VoiceRecognitionClientAlgNode::hriFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback)
00124 {
00125
00126
00127 bool feedback_is_ok = true;
00128
00129
00130
00131
00132
00133 if( !feedback_is_ok )
00134 {
00135 hri_client_.cancelGoal();
00136
00137 }
00138 }
00139
00140 void VoiceRecognitionClientAlgNode::voice_recognitionDone(const actionlib::SimpleClientGoalState& state, const iri_perception_msgs::voiceRecognitionResultConstPtr& result)
00141 {
00142 if( state.toString().compare("SUCCEEDED") == 0 )
00143 ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionDone: Goal Achieved!");
00144 else
00145 ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionDone: %s", state.toString().c_str());
00146
00147
00148 this->answer_ = result->answer;
00149 this->code_ = result->code;
00150 ROS_INFO("VoiceRecognitionClientAlgNode:: result, code: %s, %i", this->answer_.c_str(), this->code_);
00151 this->voice_recognition_action_in_progress = false;
00152 this->voice_recognition_done = true;
00153 }
00154
00155 void VoiceRecognitionClientAlgNode::voice_recognitionActive()
00156 {
00157
00158 }
00159
00160 void VoiceRecognitionClientAlgNode::voice_recognitionFeedback(const iri_perception_msgs::voiceRecognitionFeedbackConstPtr& feedback)
00161 {
00162
00163
00164 bool feedback_is_ok = true;
00165
00166
00167
00168 if(feedback->status != this->lastStatus_)
00169 {
00170 this->code_ = feedback->status;
00171 ROS_INFO("VoiceRecognitionClientAlgNode:: New status: %i", feedback->status);
00172 this->lastStatus_ = feedback->status;
00173
00174 }
00175
00176
00177 if( !feedback_is_ok )
00178 {
00179 voice_recognition_client_.cancelGoal();
00180
00181 }
00182 }
00183
00184
00185 void VoiceRecognitionClientAlgNode::hriMakeActionRequest()
00186 {
00187 ROS_INFO("VoiceRecognitionClientAlgNode::hriMakeActionRequest: Starting New Request!");
00188
00189
00190
00191 hri_client_.waitForServer();
00192 ROS_INFO("VoiceRecognitionClientAlgNode::hriMakeActionRequest: Server is Available!");
00193
00194
00195
00196 hri_goal_.sequence_file[TTS_] = this->answer_;
00197
00198 hri_client_.sendGoal(hri_goal_,
00199 boost::bind(&VoiceRecognitionClientAlgNode::hriDone, this, _1, _2),
00200 boost::bind(&VoiceRecognitionClientAlgNode::hriActive, this),
00201 boost::bind(&VoiceRecognitionClientAlgNode::hriFeedback, this, _1));
00202 ROS_INFO("VoiceRecognitionClientAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!");
00203 this->hri_action_in_progress = true;
00204 }
00205
00206 void VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest()
00207 {
00208 ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest: Starting New Request!");
00209
00210
00211
00212 voice_recognition_client_.waitForServer();
00213 ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest: Server is Available!");
00214
00215
00216
00217 voice_recognition_client_.sendGoal(voice_recognition_goal_,
00218 boost::bind(&VoiceRecognitionClientAlgNode::voice_recognitionDone, this, _1, _2),
00219 boost::bind(&VoiceRecognitionClientAlgNode::voice_recognitionActive, this),
00220 boost::bind(&VoiceRecognitionClientAlgNode::voice_recognitionFeedback, this, _1));
00221 ROS_INFO("VoiceRecognitionClientAlgNode::voice_recognitionMakeActionRequest: Goal Sent. Wait for Result!");
00222 this->voice_recognition_action_in_progress = true;
00223 }
00224
00225 void VoiceRecognitionClientAlgNode::node_config_update(Config &config, uint32_t level)
00226 {
00227 this->alg_.lock();
00228
00229 this->alg_.unlock();
00230 }
00231
00232 void VoiceRecognitionClientAlgNode::addNodeDiagnostics(void)
00233 {
00234 }
00235
00236
00237 int main(int argc,char *argv[])
00238 {
00239 return algorithm_base::main<VoiceRecognitionClientAlgNode>(argc, argv, "voice_recognition_client_alg_node");
00240 }