iwakishi.cpp
Go to the documentation of this file.
00001 /*****************************************************************************
00002  * PROJECT: iwaki
00003  *
00004  * FILE: iwakishi.cpp
00005  *
00006  * ABSTRACT: This is a ROS wrapper around Iwaki interaction manager library.
00007  *
00008  * EXAMPLE COMMAND LINE ARGUMENTS:  -t 0.1 -d DEBUG4 -l log1
00009  *                       -p PROJECTDIR/soundboard/scripts
00010  *                       -i initialize_im.georgi.xml -x
00011  *
00012  * Iwakishi: a ROS wrapper around Iwaki interaction manager library.
00013  * Copyright (C) 2012-2013 Maxim Makatchev.
00014  * 
00015  * This program is free software: you can redistribute it and/or modify
00016  * it under the terms of the GNU General Public License as published by
00017  * the Free Software Foundation, either version 3 of the License, or
00018  * (at your option) any later version.
00019  *
00020  * This program is distributed in the hope that it will be useful,
00021  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00022  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023  * GNU General Public License for more details.
00024  *
00025  * You should have received a copy of the GNU General Public License
00026  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00027  * 
00028  * 
00029  ****************************************************************/
00030 
00031 #include <iostream>
00032 #include <sstream>
00033 #include "iwakishi.h"
00034 #include "log.h"
00035 #include <string>
00036 #include <csignal>
00037 #include <algorithm>
00038 #include <ncursesw/ncurses.h> /* for getch() */
00039 
00040 #include <sys/time.h>
00041 #include <stdio.h>
00042 #include <stdlib.h>
00043 #include <getopt.h>
00044      
00045 #include <math.h>
00046 
00047 #include "ros/ros.h"
00048 #include "std_msgs/String.h"
00049 #include "iwaki/ActionMsg.h"
00050 #include "iwaki/AtomMsg.h"
00051 #include "iwaki/ActionStatusMsg.h"
00052 
00053 #include <sstream>
00054 
00055 
00056 
00061 InteractionManager im;
00062 TextUI textUI;
00063 
00064 #define KB_ENTER int('\n')
00065 
00066 ActionStatus convertActionStatusMsgToActionStatus(const iwaki::ActionStatusMsg
00067                                                   &anActionStatusMsg) {
00068     ActionStatus newActionStatus;
00069     for (std::vector<iwaki::ArgSlot>::const_iterator argslot_it =
00070              anActionStatusMsg.return_args.begin();
00071          argslot_it != anActionStatusMsg.return_args.end(); argslot_it++) {
00072         ArgSlot anArgSlot;
00073         anArgSlot.name = argslot_it->name;
00074         anArgSlot.value = argslot_it->value;
00075         anArgSlot.default_value = argslot_it->default_value;
00076         anArgSlot.type = argslot_it->type;
00077         anArgSlot.unit = argslot_it->unit;
00078         anArgSlot.var = argslot_it->var;
00079 
00080         newActionStatus.return_args.push_back(anArgSlot);
00081     }
00082 
00083     newActionStatus.action_id = anActionStatusMsg.action_id;
00084     newActionStatus.executor = anActionStatusMsg.executor;
00085     newActionStatus.status = anActionStatusMsg.status;
00086     return newActionStatus;
00087 }
00088 
00089 
00090 /*
00091  * inputAtomCallback implements the callback on the subscribed topic,
00092  * the inputs to the Iwakishi interaction manager.
00093  * */
00094 
00095 void actionStatusCallback(const iwaki::ActionStatusMsg::ConstPtr& anActionStatusMsg_p) {
00096     ActionStatus newActionStatus = convertActionStatusMsgToActionStatus(*anActionStatusMsg_p);
00097     im.processActionCompletionStatus(newActionStatus);
00098 }
00099 
00100 
00101 
00102 string makeSnippetFromAtom(Atom anAtom) {
00103     string snippet;
00104     
00105     for (std::list<VarSlot>::iterator varslot_it = anAtom.varslots.begin();
00106          varslot_it != anAtom.varslots.end(); varslot_it++) {
00107         if (varslot_it->name == "uu_string") {
00108             snippet += varslot_it->val;
00109         }
00110     }
00111     return snippet;
00112 }
00113 
00114 Atom convertAtomMsgToAtom(const iwaki::AtomMsg &anAtomMsg) {
00115     Atom new_atom;
00116         /* convert the varslots */
00117     for (std::vector<iwaki::VarSlot>::const_iterator varslot_it = anAtomMsg.varslots.begin();
00118          varslot_it != anAtomMsg.varslots.end(); varslot_it++) {
00119         VarSlot newVarSlot;
00120         newVarSlot.name = varslot_it->name;
00121         newVarSlot.val = varslot_it->val;
00122         newVarSlot.relation = varslot_it->relation;
00123         newVarSlot.type = varslot_it->type;
00124         newVarSlot.unique_mask = varslot_it->unique_mask;
00125 
00126         new_atom.varslots.push_back(newVarSlot);
00127     }
00128     return new_atom;
00129 }
00130 
00131 
00132 /*
00133  * inputAtomCallback implements the callback on the subscribed topic,
00134  * the inputs to the Iwakishi interaction manager.
00135  * */
00136 
00137 void inputAtomCallback(const iwaki::AtomMsg::ConstPtr& anAtomMsg_p) {
00138     Atom new_atom = convertAtomMsgToAtom(*anAtomMsg_p);   
00139     textUI.push_msg("INFO: received message: " + makeSnippetFromAtom(new_atom));
00140     im.input_queue.push_back(new_atom);
00141        
00142 }
00143 
00144 
00145 
00146 iwaki::ActionMsg convertActionToActionMsg(Action &anAction) {
00147 
00148     iwaki::ActionMsg anActionMsg;
00149     
00150         /* convert the args*/
00151     for (std::list<ArgSlot>::iterator arg_it = anAction.args.begin();
00152          arg_it != anAction.args.end(); arg_it++) {
00153         iwaki::ArgSlot anArgSlot;
00154         anArgSlot.name = arg_it->name;
00155         anArgSlot.value = arg_it->value;
00156         anArgSlot.default_value = arg_it->default_value;
00157         anArgSlot.type = arg_it->type;
00158         anArgSlot.unit = arg_it->unit;
00159         anArgSlot.var = arg_it->var;  // var is not really used for action slots
00160 
00161         anActionMsg.args.push_back(anArgSlot);
00162     }
00163 
00164             /* convert the return args*/
00165     for (std::list<ArgSlot>::iterator arg_it = anAction.return_args.begin();
00166          arg_it != anAction.return_args.end(); arg_it++) {
00167         iwaki::ArgSlot anArgSlot;
00168         anArgSlot.name = arg_it->name;
00169         anArgSlot.value = arg_it->value;
00170         anArgSlot.default_value = arg_it->default_value;
00171         anArgSlot.type = arg_it->type;
00172         anArgSlot.unit = arg_it->unit;
00173         anArgSlot.var = arg_it->var; // var is not really used for action return slots
00174 
00175         anActionMsg.return_args.push_back(anArgSlot);
00176     }
00177 
00178     anActionMsg.name = anAction.name;
00179     anActionMsg.id = anAction.id;
00180     anActionMsg.actor = anAction.actor;
00181         /* wierd, having a msg field called timeout gives an error
00182          * no field named wtimeout. Usimg thetimeout instead */
00183     anActionMsg.thetimeout = anAction.timeout;
00184     anActionMsg.if_timeout = anAction.if_timeout;
00185     anActionMsg.priority = anAction.priority;
00186     
00187     return anActionMsg;
00188 }
00189 
00190 /*
00191  * publishAction(Action)
00192  * repacks an Action as a ROS message and publishes it
00193  * */
00194 
00195 void publishAction(Action &anAction, ros::Publisher &action_pub) {
00196         /* Create and stuff message object */
00197     
00198     iwaki::ActionMsg anActionMsg = convertActionToActionMsg(anAction);
00199     
00200     FILE_LOG(logDEBUG1) << "Publishing ROS ActionMsg named: " << anActionMsg.name;
00201     FILE_LOG(logDEBUG1) << "                            id: " << anActionMsg.id;
00202     FILE_LOG(logDEBUG1) << "                         actor: " << anActionMsg.actor;
00203         //ROS_INFO("%d", anActionMsg.id);
00204     
00211     action_pub.publish(anActionMsg);
00212 
00213 }
00214 
00215 /*
00216  * Process the output Action queue.
00217  * */
00218 
00219 void dispatchActionsFromOutputQueue(ros::Publisher &action_pub) {
00220 
00221     std::list<Action>::iterator action_it = im.output_queue.begin();
00222 
00223     FILE_LOG(logDEBUG4) << "Action queue has length: " <<
00224         im.output_queue.size();
00225     while( action_it != im.output_queue.end() ) {
00226             /* hndAction:
00227              * here we dispatch the action from the queue.
00228              * Generally, since an action can take time longer than allowed
00229              * for the tick of the main loop,
00230              * hndAction and hndActionCompletionStatus
00231              * should be done asynchronously from this process. */
00232         publishAction(*action_it, action_pub);
00233         action_it++;
00234         im.output_queue.pop_front();
00235     }
00236 }
00237 
00238 
00239 void updateKbBuffer(string &kb_buffer, int ch) {
00240     char buffer[8];
00241     sprintf (buffer, "%c", ch);
00242     if ((ch >= 32) && (ch <= 126)) {
00243             /* legal visible characters */
00244         kb_buffer += string(buffer);
00245     } else if (ch == KEY_BACKSPACE) {
00246             /* apparently this symbol is already defined */
00247         if (!kb_buffer.empty()) {
00248             kb_buffer = kb_buffer.substr(0, kb_buffer.size() - 1);
00249         }
00250     } else if (ch == KB_ENTER) {
00251 
00252             /* hndUserInput: ignore for this app */
00253             // hndUserInput(kb_buffer);
00254         kb_buffer = "";
00255     }
00256 }
00257 
00258 
00262 int main(int argc, char **argv)
00263 {
00264     std::string logfile_name;
00265     bool text_ui = false;
00266     std::string debug_level;
00267     unsigned int timer_period_microsec = 1000000;
00268     int c;
00269     UICommand ui_command;
00270     int ch;             /* char for runtime keyboard input */
00271     string kb_buffer;   /* the keyboard buffer master copy */
00272 
00273         /* initializing ROS and parsing ROS args before app's args */
00274     ros::init(argc, argv, "iwakishi");
00275 
00276         /* do our own parsing of argument line */
00277     opterr = 0;
00278     char* cvalue;
00279     while (1)
00280     {
00281         static struct option long_options[] =
00282             {
00283                   /* These options set a flag. */
00284                   //{"verbose", no_argument,       &verbose_flag, 1},
00285                   //{"brief",   no_argument,       &verbose_flag, 0},
00286                   /* These options don't set a flag.
00287                      We distinguish them by their indices. */
00288                 {"help",    no_argument,       0, 'h'},
00289                 {"text_ui",    no_argument,       0, 'x'},
00290                 {"log", required_argument, 0, 'l'},
00291                 {"debug",   required_argument, 0, 'd'},
00292                 {"timer",   required_argument, 0, 't'},
00293                 {"init",       required_argument, 0, 'i'},
00294                 {"atoms",       required_argument, 0, 'a'},
00295                 {"path",       required_argument, 0, 'p'},
00296                 {0, 0, 0, 0}
00297           };
00298             /* getopt_long stores the option index here. */
00299         int option_index = 0;
00300         
00301         c = getopt_long (argc, argv, "hxl:d:t:i:a:p:",
00302                          long_options, &option_index);
00303         
00304           /* Detect the end of the options. */
00305         if (c == -1)
00306             break;
00307         
00308         switch (c) {
00309             case 0:
00310                     /* If this option set a flag, do nothing else now. */
00311                 if (long_options[option_index].flag != 0)
00312                     break;
00313                 printf ("option %s", long_options[option_index].name);
00314                 if (optarg)
00315                     printf (" with arg %s", optarg);
00316                 printf ("\n");
00317                 break;
00318                 
00319             case 'h':
00320                 cout << "\
00321 Usage: imcore [OPTION]... \n                    \
00322 ";
00323                 cout << "Mandatory arguments to long options are mandatory for short options too.\n\
00324 ";
00325                 cout << "\
00326   -d --debug DEBUG_LEVEL     DEBUG_LEVEL is one of the \n               \
00327                              following: ERROR, WARNING, INFO, \n        \
00328                              DEBUG, DEBUG1, DEBUG2, DEBUG3, DEBUG4\n\
00329 ";
00330                 cout << "\
00331   -t --timer TIMER_PERIOD    if present, this option causes \n          \
00332                              IM to run in timer mode, with every TIMER_PERIOD\n \
00333                              seconds IM state update is forced. If the option is\n \
00334                              not present IM runs in callback mode\n     \
00335 ";
00336                 cout << "\
00337   -l --log LOG_FILE          if present, this option causes \n          \
00338                              all the debug output be directed to the LOG_FILE\n \
00339 ";
00340                 cout << "\
00341   -i --init INIT_FILE        name of the IM init file that overrides\n  \
00342                              default initialize_im.xml\n                \
00343 ";
00344                 cout << "\
00345   -a --atoms DEFAULTS_FILE   name of the atom defaults file that overrides\n \
00346                              default default_atoms.xml\n                \
00347 ";
00348                 cout << "\
00349   -p --path PATH             path to recipes, actions and functions directories\n \
00350 ";
00351                 cout << "\
00352   -x --text_ui               turn on text ui. will supress normal debug and error \n \
00353                              terminal output\n                          \
00354 ";
00355                 
00356                 cout << "\
00357   -h --help                  This info." << endl;
00358                 return -1;
00359             case 'x':
00360                 text_ui = true;
00361                 break;
00362             case 'd':
00363                 cvalue = optarg;
00364                 debug_level=string((const char*) cvalue);
00365                 break;
00366             case 'l':
00367             {
00368                 cvalue = optarg;
00369                 logfile_name = (string)((const char*) cvalue);
00370                 if (!setLogFile(logfile_name)) {
00371                     return -1;
00372                 }
00373                 break;
00374             }
00375             case 't':
00376             {
00377                 cvalue = optarg;
00378                 std::string  cvalue_str = string((const char*) cvalue);
00379                 double timer_period_sec = string_to_double(cvalue_str);
00380                 if ((timer_period_sec > 1.0)||(timer_period_sec <= 0)) {
00381                     cout << "Timer period should be positive and not more \
00382                     than 1.0 seconds." << endl;
00383                     return -1;
00384                 }
00385                 timer_period_microsec =
00386                     (unsigned int) floor(timer_period_sec*1000000);
00387                 break;
00388             }
00389             case 'i':
00390             {
00391                 cvalue = optarg;
00392                 im.init_file_name = (string) ((const char*) cvalue);
00393                 break;
00394             }
00395             case 'a':
00396             {
00397                 cvalue = optarg;
00398                 im.default_atoms_file_name = (string) ((const char*) cvalue);
00399                 break;
00400             }
00401             case 'p':
00402             {
00403                 cvalue = optarg;
00404                 im.script_path = (string) ((const char*) cvalue);
00405                 break;
00406             }
00407             default:
00408                 cout <<
00409                     "Unknown option. Try 'soundboard --help' for more information.\n";
00410                 return -1;
00411         }
00412     }
00413   
00414         /* Print any remaining command line arguments (not options). */
00415     if (optind < argc)
00416     {
00417         printf ("non-option ARGV-elements: ");
00418         while (optind < argc)
00419             printf ("%s ", argv[optind++]);
00420         putchar ('\n');
00421     }
00422     
00423         /* Set log file reporting level */
00424     FILELog::ReportingLevel() =
00425         FILELog::FromString(debug_level.empty() ? "DEBUG1" : debug_level);
00426     
00427     
00428         /* set IM params */
00429     im.timer_period_microsec = timer_period_microsec;
00430     
00431     if (!im.initialize())
00432     {
00433         FILE_LOG(logERROR)  << "Initialization of IM failed.";
00434     } else {
00435         FILE_LOG(logINFO) << "Initialization of IM succeeded.";
00436     }
00437         //im.printRecipes();
00438         //im.printTriggerables();
00439     
00440         /* initialize text UI */
00441     if ( text_ui ) {
00442         textUI.init();
00443     }
00444     
00445     FILE_LOG(logINFO) << "Running timer with the period of " <<
00446         timer_period_microsec << " microseconds." << endl;
00447     
00448         /* ROS stuff */
00449     ros::NodeHandle n;
00450     ros::Publisher action_pub = n.advertise<iwaki::ActionMsg>("IwakiAction", 1000);
00451     ros::Subscriber atom_sub = n.subscribe("IwakiInputAtom", 1000, inputAtomCallback);
00452     ros::Subscriber actionStatus_sub = n.subscribe("IwakiActionStatus", 1000, actionStatusCallback);
00453 
00454         /* sleep to allow subscribers to establish the connection.
00455          * Inherent ROS feature, otherwise the first message can be missed. */  
00456     usleep(1000000.0);
00457     
00458         //ros::Rate loop_rate(10); /* we handle timing by ourselves */
00459         /******************************
00460          *
00461          * Main loop
00462          * 
00463          ******************************/
00464     
00465     while (ros::ok())
00466     {
00467         
00468         double start_tick = getSystemTimeMSec()/1000.0;
00469         FILE_LOG(logDEBUG4) <<
00470             "############ Re-entering the main loop at: "
00471                             << setprecision(20) << start_tick ;
00472         
00473         im.doTick();
00474         
00475             /* Dispatch actions collected at the output queue */
00476         dispatchActionsFromOutputQueue(action_pub);
00477         
00478         
00479             /* Process keyboard input */
00480         ch = getch();
00481         
00482         updateKbBuffer(kb_buffer, ch);
00483             /* if running with text_ui, do the terminal screen update */
00484         if (text_ui) {
00485                 /* pass kb_buffer to textUI for printing */
00486             textUI.keyboardBuffer = kb_buffer;  
00487             ui_command =  textUI.update(im, ch);
00488             if ( ui_command == uiQuit) {
00489                 textUI.close();
00490                 FILE_LOG(logINFO) << "TextUI requested quit. Goodbye.";
00491                 return -1;
00492             }
00493         }
00494         
00495             /*
00496              * Logging
00497              * */
00498         
00499             /* check if need to swap log file */
00500         FILE* pStream = Output2FILE::Stream();
00501             // fseek( pStream, 0, SEEK_END );
00502         int logfile_size = ftell( pStream );
00503         FILE_LOG(logDEBUG3) << "Logfile size: " << logfile_size;
00504         if (logfile_size > MAX_LOGFILE_SIZE ) {
00505                 /* move on to a new logfile */
00506             if (!setLogFile(logfile_name)) {
00507                 return -1;
00508             }
00509         }
00510         
00511         double end_tick = getSystemTimeMSec()/1000.0;
00512         
00513         FILE_LOG(logDEBUG4) <<
00514             "############ Done tick at: " << setprecision(20) <<
00515             end_tick ;
00516         
00517         FILE_LOG(logDEBUG4) <<
00518             "############ Cycle took: " << setprecision(20) <<
00519             (end_tick - start_tick) << " seconds.";
00520         
00522         if (im.timing_dump_counter == 0) {
00523             FILE_LOG(logINFO) << "Print subcycle timings...";
00524             for (std::map<string, std::list<double> >::const_iterator
00525                      map_it = im.subcycle_timings.begin();
00526                  map_it != im.subcycle_timings.end(); map_it++) {
00527                 string timings_str;
00528                 
00529                 for (std::list<double>::const_iterator
00530                          list_it = map_it->second.begin();
00531                      list_it != map_it->second.end(); list_it++) {
00532                     timings_str = timings_str + to_string(*list_it) + ", ";
00533                 }
00534                 
00535                 FILE_LOG(logINFO) << "Timing subcycle " << map_it->first
00536                                   << ": " << timings_str;
00537             }
00538             im.ptree.print(logINFO);
00539             im.getGlobalBindings()->print(logINFO);
00540             FILE_LOG(logINFO) << "Print subcycle timings...";   
00541         }
00542         
00543         if ((end_tick - start_tick) < (timer_period_microsec/1000000.0)) {
00544             FILE_LOG(logDEBUG4) <<
00545                 "########### intentially wasting: " << setprecision(20) <<
00546                 ((timer_period_microsec/1000000.0) - (end_tick - start_tick)) <<
00547                 " seconds."; 
00548             usleep(timer_period_microsec - (end_tick - start_tick)*1000000.0);
00549         } else {
00550             FILE_LOG(logWARNING) <<
00551                 "########### sampling period is short of: " << setprecision(20) <<
00552                 ((end_tick - start_tick) - (timer_period_microsec/1000000.0)) <<
00553                 " seconds.";
00554             im.ptree.print(logWARNING);
00555             im.getGlobalBindings()->print(logWARNING);
00556         }
00557         
00558             /*
00559              * End logging
00560              * */  
00561         
00562         ros::spinOnce();
00563         
00564             //loop_rate.sleep();
00565     }
00566     
00567     
00568     return 0;
00569 }


iwaki
Author(s): Maxim Makatchev, Reid Simmons
autogenerated on Mon Oct 6 2014 01:02:38