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