iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Marco Randazzo
4  * email: marco.randazzo@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
75 #include <yarp/os/Network.h>
76 #include <yarp/os/RFModule.h>
77 #include <yarp/os/PeriodicThread.h>
78 #include <yarp/os/Bottle.h>
79 #include <yarp/os/BufferedPort.h>
80 #include <yarp/os/Time.h>
81 #include <yarp/os/Log.h>
82 #include <yarp/os/LogStream.h>
83 #include <yarp/sig/Vector.h>
84 #include <yarp/math/Math.h>
85 
86 #include <yarp/dev/Drivers.h>
87 #include <yarp/dev/CartesianControl.h>
88 #include <yarp/dev/PolyDriver.h>
89 
90 //#include <gsl/gsl_math.h>
91 
92 #include <iostream>
93 #include <iomanip>
94 #include <string>
95 #include <math.h>
96 #include <SDL.h>
97 
98 using namespace std;
99 using namespace yarp::os;
100 using namespace yarp::dev;
101 using namespace yarp::sig;
102 using namespace yarp::math;
103 
104 #define PRINT_STATUS_PER 0.1
105 #define MAX_AXES 32
106 
108 
109 class CtrlThread: public PeriodicThread
110 {
111  struct struct_jointProperties
112  {
113  int type;
114  int param[3];
115  string param_s;
116  };
117 
118 protected:
119  ResourceFinder &rf;
120  BufferedPort<Bottle> port_command;
121  BufferedPort<Bottle> port_axis_only;
122  BufferedPort<Bottle> port_buttons_only;
123  bool silent;
124  bool force_cfg;
125 
126  double defaultExecTime;
127  double t0;
128  int numAxes;
129  int numBalls;
130  int numHats;
132  int joy_id;
133  SDL_Joystick* joy1;
134 
135  //button actions;
136  string button_actions [20];
137 
138  //hats actions;
139  string hat_actions [20];
140 
141  //variables read from the joystick
144  int* rawHats;
146  double* rawAxes;
147  double* outAxes;
148 
149  //variables read from the configuration file
152  double* inputMax;
153  double* inputMin;
154  double* outputMax;
155  double* outputMin;
156  double* jointDeadband;
157  int* reverse;
158  struct_jointProperties* jointProperties;
159 
160 public:
161  CtrlThread(unsigned int _period, ResourceFinder &_rf) :
162  PeriodicThread((double)_period/1000.0), rf(_rf)
163  {
164  joy_id=0;
165  rawButtons=0;
166  rawButtonsOld=0;
167  rawAxes=0;
168  rawHats=0;
169  rawHatsOld=0;
170  outAxes=0;
171  joy1=0;
172  num_inputs=0;
173  num_outputs=0;
174  inputMax=0;
175  inputMin=0;
176  outputMax=0;
177  outputMin=0;
178  jointDeadband=0;
179  reverse=0;
180  jointProperties=0;
181  silent = false;
182  force_cfg = false;
183  }
184 
185  virtual bool threadInit()
186  {
187  // get params from the RF
188  if (rf.check("silent"))
189  {
190  yInfo ("Running in silent mode\n");
191  silent=true;
192  }
193 
194  if (rf.check("force_configuration"))
195  {
196  yInfo ( "Force configuration option found\n");
197  force_cfg=true;
198  }
199 
200  if (rf.findGroup("INPUTS").check("InputsNumber"))
201  {
202  num_inputs = rf.findGroup("INPUTS").find("InputsNumber").asInt32();
203  inputMax = new double [num_inputs];
204  inputMin = new double [num_inputs];
205  outputMax = new double [num_inputs];
206  outputMin = new double [num_inputs];
207  jointDeadband = new double [num_inputs];
208  reverse = new int [num_inputs];
209  yDebug ("Number of input axes in the configuration options: %d \n",num_inputs);
210  }
211  else
212  {
213  yError ("Unable to find number of input axes in the configuration options\n");
214  return false;
215  }
216 
217  Bottle b;
218 
219 
220  b = rf.findGroup("INPUTS").findGroup("Reverse");
221  if (b.size()-1 == num_inputs)
222  {
223  for (int i = 1; i < b.size(); i++) reverse[i-1] = b.get(i).asInt32();
224  }
225  else {yError ( "Configuration error: invalid number of entries 'Reverse'\n"); return false;}
226  b = rf.findGroup("INPUTS").findGroup("InputMax");
227  if (b.size()-1 == num_inputs)
228  {
229  for (int i = 1; i < b.size(); i++) inputMax[i-1] = b.get(i).asFloat64();
230  }
231  else {yError ( "Configuration error: invalid number of entries 'InputMax'\n"); return false;}
232  b = rf.findGroup("INPUTS").findGroup("InputMin");
233  if (b.size()-1 == num_inputs)
234  {
235  for (int i = 1; i < b.size(); i++) inputMin[i-1] = b.get(i).asFloat64();
236  }
237  else {yError ( "Configuration error: invalid number of entries 'InputMin'\n"); return false;}
238  b = rf.findGroup("INPUTS").findGroup("OutputMax");
239  if (b.size()-1 == num_inputs)
240  {
241  for (int i = 1; i < b.size(); i++) outputMax[i-1] = b.get(i).asFloat64();
242  }
243  else {yError ( "Configuration error: invalid number of entries 'OutputMax'\n"); return false;}
244  b = rf.findGroup("INPUTS").findGroup("OutputMin");
245  if (b.size()-1 == num_inputs)
246  {
247  for (int i = 1; i < b.size(); i++) outputMin[i-1] = b.get(i).asFloat64();
248  }
249  else {yError ( "Configuration error: invalid number of entries 'OutputMin'\n"); return false;}
250 
251  b = rf.findGroup("INPUTS").findGroup("Deadband");
252  if (b.size()-1 == num_inputs)
253  {
254  for (int i = 1; i < b.size(); i++) jointDeadband[i-1] = b.get(i).asFloat64();
255  }
256  else {yError ( "Configuration error: invalid number of entries 'Deadband'\n"); return false;}
257 
258  if (rf.findGroup("OUTPUTS").check("OutputsNumber"))
259  {
260  num_outputs = rf.findGroup("OUTPUTS").find("OutputsNumber").asInt32();
261  jointProperties = new struct_jointProperties [num_outputs];
262  }
263  else
264  {
265  yError ( "Unable to find number of output axes in the configuration options\n");
266  return false;
267  }
268 
269  for (int i=0; i<num_outputs; i++)
270  {
271  char tmp [20];
272  sprintf (tmp,"Ax%d",i);
273  if (!rf.findGroup("OUTPUTS").check(tmp))
274  {
275  yError ( "Error reading [OUTPUT] block, unable to find Ax%d identifier\n",i);
276  return false;
277  }
278  b = rf.findGroup("OUTPUTS").findGroup(tmp);
279  if (b.get(1).asString()=="polar_r_theta")
280  {
281  jointProperties[i].type=JTYPE_POLAR;
282  jointProperties[i].param[0]=b.get(2).asInt32();
283  jointProperties[i].param[1]=b.get(3).asInt32();
284  jointProperties[i].param[2]=b.get(4).asInt32();
285  }
286  else
287  if (b.get(1).asString()=="cartesian_xyz")
288  {
289  jointProperties[i].type=JTYPE_CARTESIAN;
290  jointProperties[i].param[0]=b.get(2).asInt32();
291  jointProperties[i].param[1]=0;
292  jointProperties[i].param[2]=0;
293  }
294  else
295  if (b.get(1).asString()=="constant")
296  {
297  jointProperties[i].type=JTYPE_CONSTANT;
298  jointProperties[i].param[0]=b.get(2).asInt32();
299  jointProperties[i].param[1]=0;
300  jointProperties[i].param[2]=0;
301  }
302  else
303  if (b.get(1).asString()=="string")
304  {
305  jointProperties[i].type=JTYPE_STRING;
306  jointProperties[i].param[0]=0;
307  jointProperties[i].param[1]=0;
308  jointProperties[i].param[2]=0;
309  jointProperties[i].param_s=b.get(2).asString();
310  }
311  else
312  {
313  yError() << "Unknown [OUTPUT] property";
314  return false;
315  }
316  }
317  //string s = b.toString(); //this causes compilation issues under linux
318 
319  // open the output port
320  string output_port_name;
321  if (rf.findGroup("GENERAL").check("outputPortName"))
322  {
323  output_port_name = rf.findGroup("GENERAL").find("outputPortName").asString();
324  }
325  else
326  {
327  output_port_name = "/joystickCtrl:o";
328  yWarning ( "outputPortName not found, using %s \n", output_port_name.c_str());
329  }
330  bool ret=true;
331  ret &= port_command.open(output_port_name.c_str());
332  //@@@ TO BE COMPLETED: port name prefix to be set in the ini file
333  ret &= port_axis_only.open("/joystickCtrl/raw_axis:o");
334  ret &= port_buttons_only.open("/joystickCtrl/raw_buttons:o");
335  if (ret==false)
336  {
337  yError() << "Unable to open module ports";
338  return false;
339  }
340 
341  //get the list of the commands to be executed with the buttons
342  Bottle& exec_comm_bottle = rf.findGroup("BUTTONS_EXECUTE");
343  if (!exec_comm_bottle.isNull())
344  {
345  yInfo ( "associating the following actions to the buttons: \n");
346  for (int iii = 0; iii < 20; iii++){
347  char tmp[80];
348  sprintf(tmp, "button%d", iii);
349  if (exec_comm_bottle.check(tmp))
350  {
351  button_actions[iii] = exec_comm_bottle.find(tmp).toString();
352  printf ("%s %s\n", tmp, button_actions[iii].c_str());
353  }
354  }
355  printf ("\n");
356  }
357 
358  //get the list of the commands to be executed with the hats
359  Bottle& hats_exec_bottle = rf.findGroup("HATS_EXECUTE");
360  if (!hats_exec_bottle.isNull())
361  {
362  yInfo ( "associating the following actions to the hats: \n");
363  for (int iii = 0; iii < 20; iii++){
364  char tmp[80];
365  sprintf(tmp, "hat%d", iii);
366  if (hats_exec_bottle.check(tmp))
367  {
368  hat_actions[iii] = hats_exec_bottle.find(tmp).toString();
369  printf ("%s %s\n", tmp, hat_actions[iii].c_str());
370  }
371  }
372  printf ("\n");
373  }
374 
375  // start SDL subsystem
376  //SDL_Init(SDL_INIT_VIDEO);
377  //SDL_SetVideoMode(640, 480, 16, SDL_DOUBLEBUF);
378  if ( SDL_InitSubSystem ( SDL_INIT_JOYSTICK ) < 0 )
379  {
380  yError ( "Unable to initialize Joystick: %s\n", SDL_GetError() );
381  return false;
382  }
383 
384  // get the list of available joysticks
385  fprintf ( stderr, "\n");
386  int joy_id=0;
387  int joystick_num = SDL_NumJoysticks ();
388  if (joystick_num == 0)
389  {
390  yError ( "Error: No joysticks found\n"); return false;
391  }
392  else if (joystick_num == 1)
393  {
394  joy_id=0;
395  yInfo ( "One joystick found \n");
396 #if (SDL_MAJOR_VERSION == 2)
397  yInfo ( "Using joystick: %s \n", SDL_JoystickNameForIndex(joy_id));
398 #else
399  yInfo ( "Using joystick: %s \n", SDL_JoystickName(joy_id));
400 #endif
401 
402  }
403  else
404  {
405  yInfo ( "More than one joystick found:\n");
406  for (int i=0; i<joystick_num; i++)
407  {
408 #if (SDL_MAJOR_VERSION == 2)
409  yInfo ( "%d: %s\n",i,SDL_JoystickNameForIndex(i));
410 #else
411  yInfo ( "%d: %s\n",i,SDL_JoystickName(i));
412 #endif
413  }
414  yInfo ( "\n");
415 
416  // choose between multiple joysticks
417  if (rf.findGroup("GENERAL").check("DefaultJoystickNumber"))
418  {
419  joy_id = rf.findGroup("GENERAL").find("DefaultJoystickNumber").asInt32();
420  yInfo ( "Multiple joysticks found, using #%d, as specified in the configuration options\n", joy_id);
421  }
422  else
423  {
424  yWarning ( "No default joystick specified in the configuration options\n");
425  yWarning ( "Which joystick you want to use? (choose number) \n");
426  cin >> joy_id;
427  }
428  }
429 
430  // Open the Joystick driver
431  joy1 = SDL_JoystickOpen ( joy_id );
432  if ( joy1 == NULL )
433  {
434  yError ( "Could not open joystick\n" );
435  return false;
436  }
437 
438  // Obtaining Joystick capabilities
439  numAxes = SDL_JoystickNumAxes ( joy1 );
440  numBalls = SDL_JoystickNumBalls ( joy1 );
441  numHats = SDL_JoystickNumHats ( joy1 );
442  numButtons = SDL_JoystickNumButtons ( joy1 );
443  yInfo ( "Characteristics of joy %d: \n", joy_id);
444  yInfo ( "%i Axes\n", numAxes );
445  yInfo ( "%i Balls\n", numBalls );
446  yInfo ( "%i Hats\n", numHats );
447  yInfo ( "%i Buttons\n", numButtons );
448  yInfo ( "\n");
449 
450  // check: selected joint MUST have at least one axis
451  if (numAxes<=0)
452  {
453  yError ( "Error: selected joystick has %d Axes?!\n",numAxes );
454  return false;
455  }
456 
457  if (numAxes!=num_inputs)
458  {
459  if (force_cfg == false)
460  {
461  yWarning ( "Warning: # of joystick axes (%d) differs from # of configured input axes (%d)!\n",numAxes,num_inputs );
462  yWarning ( "This probably means that your .ini file does not containt a correct configuration.\n");
463  yWarning ( "Do you want to continue anyway (y/n)?\n");
464  char input[255];
465  cin >> input;
466  if (input[0]!='y' && input[0]!='Y')
467  {
468  yInfo ( "Quitting...\n");
469  return false;
470  }
471  else
472  {
473  yWarning ( "Overriding the number of axes specified in the configuration file. Using %d axes.\n",numAxes);
474  }
475  }
476  else
477  {
478  yWarning ( "Warning: # of joystick axes (%d) differs from # of configured input axes (%d)!\n",numAxes,num_inputs );
479  yWarning ( "This probably means that your .ini file does not containt a correct configuration.\n");
480  yWarning ( "However, --force_configuration option is enabled. This will override the number of axes specified in the configuration file.\n");
481  yWarning ( "Using %d axes.\n",numAxes);
482  }
483  }
484 
485  rawAxes = new double [MAX_AXES];
486  rawHats = new int [MAX_AXES];
487  rawHatsOld = new int [MAX_AXES];
488  rawButtons = new int [MAX_AXES];
489  rawButtonsOld = new int [MAX_AXES];
490  outAxes = new double [MAX_AXES];
491 
492  /*
493  // check: selected joint MUST have at least one button
494  if (numButtons>0)
495  rawButtons=new int [numButtons];
496  else
497  {
498  printf ( "Error reading numButtons\n" );
499  return false;
500  }*/
501  return true;
502  }
503 
504  virtual void afterStart(bool s)
505  {
506  if (s)
507  yInfo ( "Thread started successfully\n");
508  else
509  yError ( "Thread did not start\n");
510 
511  t0=Time::now();
512  }
513 
514  virtual void run()
515  {
516  //check if driver is connected
517  /*
518  if (joy_id>SDL_NumJoysticks())
519  {
520  fprintf ( stderr, "Lost connection to driver!\n");
521  }
522  fprintf ( stderr, "%d\n",SDL_NumJoysticks());
523  */
524 
525  // Updates the joystick status
526  SDL_JoystickUpdate ();
527 
528  // Reading joystick data (axes/buttons...)
529  for ( int i=0; i < numButtons; ++i )
530  {
531  rawButtonsOld[i] = rawButtons[i];
532  rawButtons[i] = SDL_JoystickGetButton ( joy1, i );
533  }
534 
535  for ( int i=0; i < numHats; ++i )
536  {
537  rawHatsOld[i] = rawHats[i];
538  rawHats[i] = SDL_JoystickGetHat ( joy1, i );
539  }
540 
541  for ( int i=0; i < numAxes; ++i )
542  {
543  rawAxes[i] = (double)SDL_JoystickGetAxis ( joy1, i );
544  }
545 
546  // Formatting input data
547  for(int i=0;i<num_inputs;i++)
548  {
549  double v = rawAxes[i];
550  if (jointDeadband[i]>0)
551  {
552  if (fabs(v)<jointDeadband[i]) v=0;
553  }
554  if (reverse[i]==1)
555  v=-v;
556 
557  if (inputMax[i]==inputMin[i])
558  {
559  v = 0;
560  }
561  else
562  {
563  v = (v<inputMax[i]) ? v : inputMax[i];
564  v = (v>inputMin[i]) ? v : inputMin[i];
565  v = v - ((inputMax[i]-inputMin[i])/2+inputMin[i]);
566  v = v / (inputMax[i]-inputMin[i]);
567  v = v * (outputMax[i]-outputMin[i]);
568  v = v + ((outputMax[i]-outputMin[i])/2+outputMin[i]);
569  }
570 
571  /*
572  v = v+jointOffset[i];
573  v = v*jointGain[i];
574  v = (v<jointMax[i]) ? v : jointMax[i];
575  v = (v>jointMin[i]) ? v : jointMin[i];
576  */
577 
578  rawAxes[i]=v;
579  }
580 
581  // Sending data out on a Yarp port
582  Bottle data;
583  Bottle axis_data;
584  Bottle buttons_data;
585  for(int i=0;i<num_outputs;i++)
586  {
587  if (jointProperties[i].type == JTYPE_POLAR)
588  {
589  if (jointProperties[i].param[2] == 0)
590  {
591  outAxes[i]= atan2( (rawAxes[jointProperties[i].param[0]]),
592  (rawAxes[jointProperties[i].param[1]]) ) * 180.0 / 3.14159265;
593  }
594  else if (jointProperties[i].param[2] == 1)
595  {
596  outAxes[i]= sqrt ( pow((rawAxes[jointProperties[i].param[0]]),2)+
597  pow((rawAxes[jointProperties[i].param[1]]),2) );
598  }
599  else
600  {
601  outAxes[i]=0.0;
602  yWarning ( "Unknown parameter for JTYPE_POLAR, joint %d\n",i);
603  }
604  }
605  else if (jointProperties[i].type == JTYPE_CARTESIAN)
606  {
607  outAxes[i]=rawAxes[jointProperties[i].param[0]];
608  }
609  else if (jointProperties[i].type == JTYPE_CONSTANT)
610  {
611  outAxes[i]=(jointProperties[i].param[0]);
612  }
613  else
614  {
615  outAxes[i]=0.0;
616  yWarning ( "Unknown property, joint %d\n",i);
617  }
618  }
619 
620  //execute button actions
621  for (int i=0;i<numButtons;i++)
622  {
623  if (rawButtonsOld[i] == 0 && rawButtons[i] == 1)
624  {
625  //execute script
626  if (!button_actions[i].empty())
627  {
628  yInfo ("executing script %d: %s\n", i, button_actions[i].c_str());
629  int ret = system(button_actions[i].c_str());
630  }
631  else
632  {
633  yWarning ("no scripts associated to button %d\n", i);
634  }
635  }
636  }
637 
638  //execute button actions
639  for (int i=0;i<numHats;i++)
640  {
641  if (rawHats[i] != SDL_HAT_CENTERED)
642  {
643  //execute script
644  if (!hat_actions[i].empty())
645  {
646  string action(hat_actions[i]);
647  action += " ";
648  //if true, the behavior is like a button, one esecution per press
649  //if false, execution is continuous
650  bool trigger_mode=true;
651  switch(rawHats[i])
652  {
653  case SDL_HAT_UP:
654  action += "up";
655  break;
656  case SDL_HAT_RIGHT:
657  action += "right";
658  break;
659  case SDL_HAT_DOWN:
660  action += "down";
661  break;
662  case SDL_HAT_LEFT:
663  action += "left";
664  break;
665  case SDL_HAT_RIGHTUP:
666  action += "rightup";
667  break;
668  case SDL_HAT_RIGHTDOWN:
669  action += "rightdown";
670  break;
671  case SDL_HAT_LEFTUP:
672  action += "leftup";
673  break;
674  case SDL_HAT_LEFTDOWN:
675  action += "leftdown";
676  break;
677  default:
678  break;
679  }
680  if (trigger_mode)
681  {
682  if (rawHatsOld[i]==SDL_HAT_CENTERED)
683  {
684  yInfo ("executing script %d: %s\n", i, action.c_str());
685  int ret = system(action.c_str());
686  }
687  }
688  else
689  {
690  yInfo ("executing script %d: %s\n", i, action.c_str());
691  int ret = system(action.c_str());
692  }
693  }
694  else
695  {
696  yWarning ("no scripts associated to button %d\n", i);
697  }
698  }
699  }
700 
701  // Preparing data to be sent on the yarp ports
702  for(int i=0;i<num_outputs;i++)
703  {
704  if ( jointProperties[i].type == JTYPE_STRING)
705  data.addString(jointProperties[i].param_s.c_str());
706  else
707  data.addFloat64(outAxes[i]);
708  }
709  for (int i=0;i<numButtons;i++)
710  {
711  buttons_data.addFloat64(rawButtons[i]);
712  }
713  for (int i=0;i<numAxes; i++)
714  {
715  axis_data.addFloat64(rawAxes[i]);
716  }
717 
718  // Sending data on the yarp ports
719  if (port_command.getOutputCount()>0)
720  {
721  port_command.prepare() = data;
722  port_command.write();
723  }
724  if (port_axis_only.getOutputCount()>0)
725  {
726  port_axis_only.prepare() = axis_data;
727  port_axis_only.write();
728  }
729  if (port_buttons_only.getOutputCount()>0)
730  {
731  port_buttons_only.prepare() = buttons_data;
732  port_buttons_only.write();
733  }
734 
735  // Displaying status
736  if (!silent) printStatus();
737  }
738 
739  virtual void threadRelease()
740  {
741  if (rawAxes) delete [] rawAxes;
742  if (rawHats) delete [] rawHats;
743  if (rawHatsOld) delete [] rawHatsOld;
744  if (outAxes) delete [] outAxes;
745  if (rawButtons) delete [] rawButtons;
746  if (inputMax) delete [] inputMax;
747  if (inputMin) delete [] inputMin;
748  if (outputMax) delete [] outputMax;
749  if (outputMin) delete [] outputMin;
750  if (jointDeadband) delete [] jointDeadband;
751  if (jointProperties) delete [] jointProperties;
752  if (reverse) delete [] reverse;
753  port_command.interrupt();
754  port_command.close();
755  port_axis_only.interrupt();
756  port_axis_only.close();
757  port_buttons_only.interrupt();
758  port_buttons_only.close();
759  }
760 
761 
762  void printStatus()
763  {
764  double t=Time::now();
765  static char buff [1000];
766  buff[0] = 0;
767 
768  if (t-t0>=PRINT_STATUS_PER)
769  {
770  //for (int i=0;i <numButtons; i++)
771  // sprintf (buff, "%+6d", rawButtons[i] );
772 
773  for (int i=0;i <numAxes; i++)
774  {
775  sprintf(buff + strlen(buff), "%+9.1f", rawAxes[i]);
776  }
777  sprintf(buff + strlen(buff), " ---> ");
778  for (int i=0;i <num_outputs; i++)
779  {
780  sprintf(buff + strlen(buff), "%+9.1f", outAxes[i]);
781  }
782  yDebug() << buff;
783  t0=t;
784  }
785  }
786 };
787 
788 
789 
790 class CtrlModule: public RFModule
791 {
792 protected:
793  CtrlThread *thr;
794  //Port rpcPort;
795 
796 public:
798 
799  virtual bool configure(ResourceFinder &rf)
800  {
801  int rateThread = 10;
802  if (rf.findGroup("GENERAL").check("rateThread"))
803  {
804  rateThread = rf.findGroup("GENERAL").find("rateThread").asInt32();
805  }
806  else
807  {
808  yWarning ("rateThread option not found, assuming %d ms\n", rateThread);
809  }
810  thr=new CtrlThread(rateThread,rf);
811  if (!thr->start())
812  {
813  delete thr;
814  return false;
815  }
816 
817  //rpcPort.open("/joystickCtrl/rpc");
818  //attach(rpcPort);
819 
820  return true;
821  }
822 
823  virtual bool close()
824  {
825  thr->stop();
826  delete thr;
827 
828  //rpcPort.interrupt();
829  //rpcPort.close();
830 
831  return true;
832  }
833 
834  virtual double getPeriod() { return 1.0; }
835  virtual bool updateModule() { return true; }
836 };
837 
838 
839 
840 int main(int argc, char *argv[])
841 {
842  ResourceFinder rf;
843  rf.setDefaultConfigFile("joystickControl.ini"); //overridden by --from parameter
844  rf.setDefaultContext("joystickControl"); //overridden by --context parameter
845  rf.configure(argc,argv);
846 
847  if (rf.check("help"))
848  {
849  yInfo ( "Options:\n");
850  yInfo ( "--silent: supress text output\n");
851  yInfo ( "--force_configuration: force a joystick configuration for a joystick with differnt # of axes, buttons etc.\n");
852  return 0;
853  }
854 
855  Network yarp;
856 
857  if (!yarp.checkNetwork())
858  {
859  yError("Sorry YARP network does not seem to be available, is the yarp server available?\n");
860  return -1;
861  }
862 
863  //SDL_JoystickEventState (SDL_ENABLE);
864  // this will alter the behaviour of the event queue of the sdl system
865  SDL_JoystickEventState ( SDL_QUERY );
866 
867  CtrlModule mod;
868 
869  return mod.runModule(rf);
870 }
871 
872 
873 
@ data
virtual bool configure(ResourceFinder &rf)
Definition: main.cpp:799
virtual double getPeriod()
Definition: main.cpp:834
CtrlModule()
Definition: main.cpp:797
virtual bool updateModule()
Definition: main.cpp:835
virtual bool close()
Definition: main.cpp:823
BufferedPort< Bottle > port_buttons_only
Definition: main.cpp:122
double * rawAxes
Definition: main.cpp:146
int * rawHatsOld
Definition: main.cpp:145
virtual bool threadInit()
Definition: main.cpp:185
int * rawButtonsOld
Definition: main.cpp:143
int joy_id
Definition: main.cpp:132
SDL_Joystick * joy1
Definition: main.cpp:133
virtual void afterStart(bool s)
Definition: main.cpp:504
virtual void threadRelease()
Definition: main.cpp:739
int num_inputs
Definition: main.cpp:150
int numButtons
Definition: main.cpp:131
int numHats
Definition: main.cpp:130
struct_jointProperties * jointProperties
Definition: main.cpp:158
BufferedPort< Bottle > port_command
Definition: main.cpp:120
int * rawButtons
Definition: main.cpp:142
int * reverse
Definition: main.cpp:157
int numAxes
Definition: main.cpp:128
double * inputMin
Definition: main.cpp:153
double * outputMax
Definition: main.cpp:154
int * rawHats
Definition: main.cpp:144
CtrlThread(unsigned int _period, ResourceFinder &_rf)
Definition: main.cpp:161
BufferedPort< Bottle > port_axis_only
Definition: main.cpp:121
bool force_cfg
Definition: main.cpp:124
bool silent
Definition: main.cpp:123
double * jointDeadband
Definition: main.cpp:156
double * outputMin
Definition: main.cpp:155
double * inputMax
Definition: main.cpp:152
virtual void run()
Definition: main.cpp:514
int num_outputs
Definition: main.cpp:151
int numBalls
Definition: main.cpp:129
void printStatus()
Definition: main.cpp:762
double * outAxes
Definition: main.cpp:147
int main(int argc, char *argv[])
Definition: main.cpp:31
#define MAX_AXES
Definition: main.cpp:105
@ JTYPE_CONSTANT
Definition: main.cpp:107
@ JTYPE_UNDEF
Definition: main.cpp:107
@ JTYPE_STRING
Definition: main.cpp:107
@ JTYPE_CARTESIAN
Definition: main.cpp:107
@ JTYPE_POLAR
Definition: main.cpp:107
#define PRINT_STATUS_PER
Definition: main.cpp:104
Copyright (C) 2008 RobotCub Consortium.
fprintf(fid,'\n')