iCub-main
Loading...
Searching...
No Matches
main.cpp
Go to the documentation of this file.
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3/*
4 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
5 * Author: Francesco Nori
6 * email: francesco.nori@iit.it
7 * website: www.robotcub.org
8 * Permission is granted to copy, distribute, and/or modify this program
9 * under the terms of the GNU General Public License, version 2 or any
10 * later version published by the Free Software Foundation.
11 *
12 * A copy of the license can be found at
13 * http://www.robotcub.org/icub/license/gpl.txt
14 *
15 * This program is distributed in the hope that it will be useful, but
16 * WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
18 * Public License for more details
19*/
20
190#include <yarp/os/ResourceFinder.h>
191#include <yarp/os/RFModule.h>
192#include <yarp/os/Log.h>
193#include <yarp/os/LogStream.h>
194
195#include "dumperThread.h"
196#include <string>
197
198#define NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP 17
199#define NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP 3
200
201
202//
203void getRate(ResourceFinder &rf, int &r)
204{
205 if (!rf.check("rate"))
206 r = 500;
207 else
208 {
209 Value& rate = rf.find("rate");
210 r = rate.asInt32();
211 }
212}
213
214int getNumberUsedJoints(ResourceFinder &rf, int &n)
215{
216 if (!rf.check("joints"))
217 {
218 yError("Missing option 'joints' in given config file\n");
219 return 0;
220 }
221 Value& joints = rf.find("joints");
222 Bottle *pJoints = joints.asList();
223 if (pJoints == 0)
224 {
225 yError("Error in option 'joints'\n");
226 return 0;
227 }
228 n = pJoints->size();
229
230 return 1;
231}
232
233int getUsedJointsMap(ResourceFinder &rf, int n, int* thetaMap)
234{
235 if (!rf.check("joints"))
236 {
237 yError("Missing option 'joints' in given config file\n");
238 return 0;
239 }
240
241 Value& joints = rf.find("joints");
242 Bottle *pJoints = joints.asList();
243 if (pJoints == 0)
244 {
245 yError("Error in option 'joints'\n");
246 return 0;
247 }
248 if (pJoints->size()!=n)
249 {
250 yError("The 'nJoints' and 'joints' params are incompatible");
251 return 0;
252 }
253
254 for (int i = 0; i < n; i++)
255 {
256 thetaMap[i] = pJoints->get(i).asInt32();
257 }
258 return 1;
259}
260
261int getNumberConstraints(ResourceFinder &rf, int &n)
262{
263 if (!rf.check("nConstr"))
264 {
265 yError("Missing option 'nConstr' in given config file\n");
266 return 0;
267 }
268 Value& nJoints = rf.find("nConstr");
269 n = nJoints.asInt32();
270 return 1;
271}
272
273int getNumberDataToDump(ResourceFinder &rf, int &n)
274{
275 if (rf.check("dataToDumpAll"))
276 {
278 return 1;
279 }
280
281 if (!rf.check("dataToDump"))
282 {
283 yError("Missing option 'dataToDump' in given config file\n");
284 return 0;
285 }
286
287 Value& list = rf.find("dataToDump");
288 Bottle *pList = list.asList();
289 if (pList == 0)
290 {
291 yError("Error in option 'dataToDump'\n");
292 return 0;
293 }
294 n = pList->size();
295 return 1;
296}
297
298int getDataToDump(ResourceFinder &rf, std::string *listOfData, int n, bool *needDebug)
299{
300 std::string availableDataToDump[NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP];
301 std::string availableDebugDataToDump[NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP];
302
303 int j;
304
305 // standard
306 availableDataToDump[0] = "getEncoders";
307 availableDataToDump[1] = "getEncoderSpeeds";
308 availableDataToDump[2] = "getEncoderAccelerations";
309 availableDataToDump[3] = "getPositionErrors";
310 availableDataToDump[4] = "getOutputs";
311 availableDataToDump[5] = "getCurrents";
312 availableDataToDump[6] = "getTorques";
313 availableDataToDump[7] = "getTorqueErrors";
314 availableDataToDump[8] = "getPosPidReferences";
315 availableDataToDump[9] = "getTrqPidReferences";
316 availableDataToDump[10] = "getControlModes";
317 availableDataToDump[11] = "getInteractionModes";
318 availableDataToDump[12] = "getMotorEncoders";
319 availableDataToDump[13] = "getMotorSpeeds";
320 availableDataToDump[14] = "getMotorAccelerations";
321 availableDataToDump[15] = "getTemperatures";
322 availableDataToDump[16] = "getMotorsPwm";
323 // debug
324 availableDebugDataToDump[0] = "getRotorPositions";
325 availableDebugDataToDump[1] = "getRotorSpeeds";
326 availableDebugDataToDump[2] = "getRotorAccelerations";
327
328 if (rf.check("dataToDumpAll"))
329 {
330 for (int i = 0; i < n; i++)
331 listOfData[i] = availableDataToDump[i];
332
333 return 1;
334 }
335
336 if (!rf.check("dataToDump"))
337 {
338 yError("Missing option 'dataToDump' in given config file\n");
339 return 0;
340 }
341
342 Value& list = rf.find("dataToDump");
343 Bottle *pList = list.asList();
344
345 for (int i = 0; i < n; i++)
346 {
347 listOfData[i] = pList->get(i).toString();
348 // check if the requested data is a standard one
350 {
351 if (listOfData[i] == (availableDataToDump[j]))
352 break;
353 }
354 // check if the requested data is a debug one
355 for (j = 0; j< NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP; j++)
356 {
357 if (listOfData[i] == (availableDebugDataToDump[j]))
358 {
359 *needDebug = true;
360 break;
361 }
362 }
363 // if not found
365 {
366 yError("Illegal values for 'dataToDump': %s does not exist!\n",listOfData[i].c_str());
367 return 0;
368 }
369 }
370
371 return 1;
372}
373
374
375class DumpModule: public RFModule
376{
377private:
378 Property fileOptions;
379 Property ddBoardOptions;
380 Property ddDebugOptions;
381 std::string name;
382 std::string configFileRobotPart;
383 std::string portPrefix;
384
385 PolyDriver ddBoard;
386 PolyDriver ddDebug;
387 bool useDebugClient;
388
389 //get the joints to be dumped
390 int rate;
391 int nJoints;
392 int* thetaMap;
393 std::string *dataToDump;
394 int nData;
395
396 boardDumperThread *myDumper;
397
398 //time stamp
399 IPreciselyTimed *istmp;
400 //encoders
401 IEncoders *ienc;
402 GetSpeeds myGetSpeeds;
403 GetAccs myGetAccs;
404 GetEncs myGetEncs;
405 //motor
406 IMotor *imot;
407 GetTemps myGetTemps;
408 //motor encoders
409 IMotorEncoders *imotenc;
410 GetMotSpeeds myGetMotorSpeeds;
411 GetMotAccs myGetMotorAccs;
412 GetMotEncs myGetMotorEncs;
413 //pid
414 IPidControl *ipid;
415 GetPosErrs myGetPosErrs;
416 GetPidRefs myGetPidRefs;
417 GetOuts myGetOuts;
418 //amp
419 IAmplifierControl *iamp;
420 GetCurrs myGetCurrs;
421 GetMotPwm myGetMotPwm;
422 //torques
423 ITorqueControl *itrq;
424 GetTrqs myGetTrqs;
425 GetTrqRefs myGetTrqRefs;
426 GetTrqErrs myGetTrqErrs;
427 //modes
428 IControlMode *icmod;
429 IInteractionMode *iimod;
430 GetControlModes myGetControlModes;
431 GetInteractionModes myGetInteractionModes;
432
433public:
434 DumpModule() : useDebugClient(false)
435 {
436 istmp=0;
437 ienc=0;
438 imotenc=0;
439 ipid=0;
440 iamp=0;
441 itrq=0;
442 icmod=0;
443 iimod=0;
444 }
445
446 virtual bool configure(ResourceFinder &rf)
447 {
448 // get command line options
449 if (!rf.check("robot") || !rf.check("part"))
450 {
451 printf("Missing either --robot or --part options. Quitting!\n");
452 return false;
453 }
454
455 std::string dumpername;
456 // get dumepr name
457 if (rf.check("name"))
458 {
459 dumpername = rf.find("name").asString();
460 dumpername += "/";
461 }
462 else
463 {
464 dumpername = "/controlBoardDumper/";
465 }
466
467 Value& robot = rf.find("robot");
468 Value& part = rf.find("part");
469 configFileRobotPart = "config_";
470 configFileRobotPart = configFileRobotPart + robot.asString();
471 configFileRobotPart = configFileRobotPart + "_";
472 configFileRobotPart = configFileRobotPart + part.asString();
473 configFileRobotPart = configFileRobotPart + ".ini";
474
475 //get the joints to be dumped
476 getRate(rf, rate);
477
478 if (getNumberUsedJoints(rf, nJoints) == 0)
479 return false;
480
481 thetaMap = new int[nJoints];
482 if (getUsedJointsMap(rf, nJoints, thetaMap) == 0)
483 return false;
484
485 //get the type of data to be dumped
486 if (getNumberDataToDump(rf, nData) == 0)
487 return false;
488
489 dataToDump = new std::string[nData];
490 if (getDataToDump(rf, dataToDump, nData, &useDebugClient) == 0)
491 return false;
492
493 // Printing configuration to the user
494 yInfo("Running with the following configuration:\n");
495 yInfo("Selected rate is: %d\n", rate);
496 yInfo("Data selected to be dumped are:\n");
497 for (int i = 0; i < nData; i++)
498 yInfo("\t%s \n", dataToDump[i].c_str());
499
500 //open remote control board
501 ddBoardOptions.put("device", "remote_controlboard");
502 ddDebugOptions.put("device", "debugInterfaceClient");
503
504 std::string localPortName = name;
505 std::string localDebugPortName = name;
506 localPortName = localPortName + dumpername;
507 localDebugPortName = localPortName + "debug/";
508 //localPortName = localPortName + robot.asString();
509 localPortName = localPortName + part.asString();
510 localDebugPortName = localDebugPortName + part.asString();
511 ddBoardOptions.put("local", localPortName);
512 ddDebugOptions.put("local", localDebugPortName);
513
514 std::string remotePortName = "/";
515 std::string remoteDebugPortName;
516 remotePortName = remotePortName + robot.asString();
517 remotePortName = remotePortName + "/";
518 remotePortName = remotePortName + part.asString();
519 ddBoardOptions.put("remote", remotePortName);
520
521 remoteDebugPortName = remotePortName + "/debug";
522 ddDebugOptions.put("remote", remoteDebugPortName);
523
524 // create a device
525 ddBoard.open(ddBoardOptions);
526 if (!ddBoard.isValid()) {
527 printf("Device not available.\n");
528 return false;
529 }
530
531 if (useDebugClient )
532 {
533 ddDebug.open(ddDebugOptions);
534 if (!ddDebug.isValid())
535 {
536 yError("\n-----------------------------------------\n");
537 yError("Debug Interface is mandatory to run this module with the '--dataToDumpAll' option or to dump any of the getRotorxxx data.\n");
538 yError("Please Verify the following 2 conditions are satisfied:\n\n");
539 yError("1) Check 'debugInterfaceClient' is available using 'yarpdev --list' command\n");
540// yError("%s", Drivers::factory().toString().c_str());
541
542 std::string deviceList, myDev;
543 deviceList.clear();
544 deviceList.append(Drivers::factory().toString());
545 myDev = "debugInterfaceClient";
546 if(deviceList.find(myDev) != std::string::npos)
547 yError("\t--> Seems OK\n");
548 else
549 yError("\t--> Seems NOT OK. The device was not found, please activate the compilation using the corrisponding CMake flag.\n");
550
551 yError("\n2) Check if the robot has the 'debugInterfaceWrapper' device up and running. \n You should see from 'yarp name list' output, a port called\n");
552 yError("\t/robotName/part_name/debug/rpc:i\n If not, fix the robot configuration files to instantiate the 'debugInterfaceWrapper' device.\n");
553 yError("\nQuickFix: If you set the --dataToDumpAll and do not need the advanced debug feature (getRotorxxx) just remove this option. See help for more information.\n");
554 yError("------------- END ERROR MESSAGE ---------------\n\n");
555 return false;
556 }
557 }
558
559 bool logToFile = false;
560 if (rf.check("logToFile")) logToFile = true;
561
562 portPrefix= dumpername + part.asString() + "/";
563 //boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]);
564 //myDumper->setThetaMap(thetaMap, nJoints);
565
566 myDumper = new boardDumperThread[nData];
567
568 for (int i = 0; i < nData; i++)
569 {
570 if (dataToDump[i] == "getEncoders" )
571 {
572 if (ddBoard.view(ienc))
573 {
574 yInfo("Initializing a getEncs thread\n");
575 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
576 myDumper[i].setThetaMap(thetaMap, nJoints);
577 myGetEncs.setInterface(ienc);
578 if (ddBoard.view(istmp))
579 {
580 yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
581 myGetEncs.setStamp(istmp);
582 }
583 else
584 yError("Problems getting the time stamp interfaces \n");
585 myDumper[i].setGetter(&myGetEncs);
586 }
587 }
588 else if (dataToDump[i] == "getEncoderSpeeds")
589 {
590 if (ddBoard.view(ienc))
591 {
592 yInfo("Initializing a getSpeeds thread\n");
593 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
594 myDumper[i].setThetaMap(thetaMap, nJoints);
595 myGetSpeeds.setInterface(ienc);
596 if (ddBoard.view(istmp))
597 {
598 yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
599 myGetSpeeds.setStamp(istmp);
600 }
601 else
602 yError("Problems getting the time stamp interfaces \n");
603 myDumper[i].setGetter(&myGetSpeeds);
604 }
605 }
606 else if (dataToDump[i] == "getEncoderAccelerations")
607 {
608 if (ddBoard.view(ienc))
609 {
610 yInfo("Initializing a getAccs thread\n");
611 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
612 myDumper[i].setThetaMap(thetaMap, nJoints);
613 myGetAccs.setInterface(ienc);
614 if (ddBoard.view(istmp))
615 {
616 yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
617 myGetAccs.setStamp(istmp);
618 }
619 else
620 yError("Problems getting the time stamp interfaces \n");
621 myDumper[i].setGetter(&myGetAccs);
622 }
623 }
624 else if (dataToDump[i] == "getPosPidReferences")
625 {
626 if (ddBoard.view(ipid))
627 {
628 yInfo("Initializing a getErrs thread\n");
629 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
630 myDumper[i].setThetaMap(thetaMap, nJoints);
631 myGetPidRefs.setInterface(ipid);
632 if (ddBoard.view(istmp))
633 {
634 yInfo("getPosPidReferences::The time stamp initalization interfaces was successfull! \n");
635 myGetPidRefs.setStamp(istmp);
636 }
637 else
638 yError("Problems getting the time stamp interfaces \n");
639 myDumper[i].setGetter(&myGetPidRefs);
640 }
641 }
642 else if (dataToDump[i] == "getTrqPidReferences")
643 {
644 if (ddBoard.view(itrq))
645 {
646 yInfo("Initializing a getErrs thread\n");
647 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
648 myDumper[i].setThetaMap(thetaMap, nJoints);
649 myGetTrqRefs.setInterface(itrq);
650 if (ddBoard.view(istmp))
651 {
652 yInfo("getTrqPidReferences::The time stamp initalization interfaces was successfull! \n");
653 myGetTrqRefs.setStamp(istmp);
654 }
655 else
656 yError("Problems getting the time stamp interfaces \n");
657 myDumper[i].setGetter(&myGetTrqRefs);
658 }
659 }
660 else if (dataToDump[i] == "getControlModes")
661 {
662 if (ddBoard.view(icmod))
663 {
664 yInfo("Initializing a getErrs thread\n");
665 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
666 myDumper[i].setThetaMap(thetaMap, nJoints);
667 myGetControlModes.setInterface(icmod, nJoints);
668 if (ddBoard.view(istmp))
669 {
670 yInfo("getControlModes::The time stamp initalization interfaces was successfull! \n");
671 myGetControlModes.setStamp(istmp);
672 }
673 else
674 yError("Problems getting the time stamp interfaces \n");
675 myDumper[i].setGetter(&myGetControlModes);
676 }
677 }
678 else if (dataToDump[i] == "getInteractionModes")
679 {
680 if (ddBoard.view(iimod))
681 {
682 yInfo("Initializing a getErrs thread\n");
683 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
684 myDumper[i].setThetaMap(thetaMap, nJoints);
685 myGetInteractionModes.setInterface(iimod, nJoints);
686 if (ddBoard.view(istmp))
687 {
688 yInfo("getInteractionModes::The time stamp initalization interfaces was successfull! \n");
689 myGetInteractionModes.setStamp(istmp);
690 }
691 else
692 yError("Problems getting the time stamp interfaces \n");
693 myDumper[i].setGetter(&myGetInteractionModes);
694 }
695 }
696 else if (dataToDump[i] == "getPositionErrors")
697 {
698 if (ddBoard.view(ipid))
699 {
700 yInfo("Initializing a getErrs thread\n");
701 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
702 myDumper[i].setThetaMap(thetaMap, nJoints);
703 myGetPosErrs.setInterface(ipid);
704 if (ddBoard.view(istmp))
705 {
706 yInfo("getPositionErrors::The time stamp initalization interfaces was successfull! \n");
707 myGetPosErrs.setStamp(istmp);
708 }
709 else
710 yError("Problems getting the time stamp interfaces \n");
711 myDumper[i].setGetter(&myGetPosErrs);
712 }
713 }
714 else if (dataToDump[i] == "getOutputs")
715 {
716 if (ddBoard.view(ipid))
717 {
718 yInfo("Initializing a getOuts thread\n");
719 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
720 myDumper[i].setThetaMap(thetaMap, nJoints);
721 myGetOuts.setInterface(ipid);
722 if (ddBoard.view(istmp))
723 {
724 yInfo("getOutputs::The time stamp initalization interfaces was successfull! \n");
725 myGetOuts.setStamp(istmp);
726 }
727 else
728 yError("Problems getting the time stamp interfaces \n");
729 myDumper[i].setGetter(&myGetOuts);
730 }
731 }
732 else if (dataToDump[i] == "getCurrents")
733 {
734 if (ddBoard.view(iamp))
735 {
736 yInfo("Initializing a getCurrs thread\n");
737 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
738 myDumper[i].setThetaMap(thetaMap, nJoints);
739 myGetCurrs.setInterface(iamp);
740 if (ddBoard.view(istmp))
741 {
742 yInfo("getCurrents::The time stamp initalization interfaces was successfull! \n");
743 myGetCurrs.setStamp(istmp);
744 }
745 else
746 yError("Problems getting the time stamp interfaces \n");
747
748 myDumper[i].setGetter(&myGetCurrs);
749 }
750 }
751 else if (dataToDump[i] == "getTorques")
752 {
753 if (ddBoard.view(itrq))
754 {
755 yInfo("Initializing a getTorques thread\n");
756 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
757 myDumper[i].setThetaMap(thetaMap, nJoints);
758 myGetTrqs.setInterface(itrq);
759 if (ddBoard.view(istmp))
760 {
761 yInfo("getTorques::The time stamp initalization interfaces was successfull! \n");
762 myGetTrqs.setStamp(istmp);
763 }
764 else
765 yError("Problems getting the time stamp interfaces \n");
766
767 myDumper[i].setGetter(&myGetTrqs);
768 }
769 }
770 else if (dataToDump[i] == "getTorqueErrors")
771 {
772 if (ddBoard.view(ipid))
773 {
774 yInfo("Initializing a getTorqueErrors thread\n");
775 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
776 myDumper[i].setThetaMap(thetaMap, nJoints);
777 myGetTrqErrs.setInterface(ipid);
778 if (ddBoard.view(istmp))
779 {
780 yInfo("getTorqueErrors::The time stamp initalization interfaces was successfull! \n");
781 myGetTrqErrs.setStamp(istmp);
782 }
783 else
784 yError("Problems getting the time stamp interfaces \n");
785
786 myDumper[i].setGetter(&myGetTrqErrs);
787 }
788 }
789 else if (dataToDump[i] == "getTemperatures")
790 {
791 if (ddBoard.view(imotenc))
792 {
793 yInfo("Initializing a getTemps thread\n");
794 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
795 myDumper[i].setThetaMap(thetaMap, nJoints);
796 myGetTemps.setInterface(imot);
797 if (ddBoard.view(istmp))
798 {
799 yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
800 myGetTemps.setStamp(istmp);
801 }
802 else
803 yError("Problems getting the time stamp interfaces \n");
804 myDumper[i].setGetter(&myGetTemps);
805 }
806 }
807 else if (dataToDump[i] == "getMotorEncoders")
808 {
809 if (ddBoard.view(imotenc))
810 {
811 yInfo("Initializing a getEncs thread\n");
812 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
813 myDumper[i].setThetaMap(thetaMap, nJoints);
814 myGetMotorEncs.setInterface(imotenc);
815 if (ddBoard.view(istmp))
816 {
817 yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
818 myGetMotorEncs.setStamp(istmp);
819 }
820 else
821 yError("Problems getting the time stamp interfaces \n");
822 myDumper[i].setGetter(&myGetMotorEncs);
823 }
824 }
825 else if (dataToDump[i] == "getMotorEncoderSpeeds")
826 {
827 if (ddBoard.view(imotenc))
828 {
829 yInfo("Initializing a getSpeeds thread\n");
830 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
831 myDumper[i].setThetaMap(thetaMap, nJoints);
832 myGetMotorSpeeds.setInterface(imotenc);
833 if (ddBoard.view(istmp))
834 {
835 yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
836 myGetMotorSpeeds.setStamp(istmp);
837 }
838 else
839 yError("Problems getting the time stamp interfaces \n");
840 myDumper[i].setGetter(&myGetMotorSpeeds);
841 }
842 }
843 else if (dataToDump[i] == "getMotorEncoderAccelerations")
844 {
845 if (ddBoard.view(imotenc))
846 {
847 yInfo("Initializing a getAccs thread\n");
848 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
849 myDumper[i].setThetaMap(thetaMap, nJoints);
850 myGetMotorAccs.setInterface(imotenc);
851 if (ddBoard.view(istmp))
852 {
853 yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
854 myGetMotorAccs.setStamp(istmp);
855 }
856 else
857 yError("Problems getting the time stamp interfaces \n");
858 myDumper[i].setGetter(&myGetMotorAccs);
859 }
860 }
861 else if (dataToDump[i] == "getMotorsPwm")
862 {
863 if (ddBoard.view(iamp))
864 {
865 yInfo("Initializing a getMotPwm thread\n");
866 myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
867 myDumper[i].setThetaMap(thetaMap, nJoints);
868 myGetMotPwm.setInterface(iamp);
869 if(ienc == 0)
870 {
871 if(!ddBoard.view(ienc))
872 return false;
873 }
874 ienc->getAxes(&myGetMotPwm.n_joint_part);
875 if (ddBoard.view(istmp))
876 {
877 yInfo("getMotorsPwm::The time stamp initalization interfaces was successfull! \n");
878 myGetMotPwm.setStamp(istmp);
879 }
880 else
881 yError("Problems getting the time stamp interfaces \n");
882 myDumper[i].setGetter(&myGetMotPwm);
883 }
884 }
885 }
886 Time::delay(1);
887 for (int i = 0; i < nData; i++)
888 myDumper[i].start();
889
890 return true;
891 }
892
893 virtual bool updateModule()
894 {
895 return true;
896 }
897
898 virtual bool close()
899 {
900 yInfo("Stopping dumper class\n");
901 for(int i = 0; i < nData; i++)
902 myDumper[i].stop();
903
904 yInfo("Deleting dumper class\n");
905 delete[] myDumper;
906
907 //finally close the dd of the remote control board
908 yInfo("Closing the device driver\n");
909 if (ddBoard.isValid()) ddBoard.close();
910 yInfo("Device driver closed\n");
911
912 //finally close the dd of the debug interface clien
913 yInfo("Closing the debug interface\n");
914 if (ddDebug.isValid()) ddDebug.close();
915 yInfo("Debug interface closed\n");
916
917 delete[] thetaMap;
918 delete[] dataToDump;
919 return true;
920 }
921};
922
923
924int main(int argc, char *argv[])
925{
926 Network yarp;
927
928 ResourceFinder rf;
929 rf.setDefaultContext("controlBoardDumper");
930 rf.setDefaultConfigFile("controlBoardDumper.ini");
931
932 rf.setDefault("part","head");
933 rf.setDefault("robot","icub");
934 rf.setDefault("rate","500");
935 rf.setDefault("joints","(0)");
936 rf.setDefault("dataToDump","(getEncoders getEncoderSpeeds getEncoderAccelerations getPositionErrors getOutputs getCurrents getTorques getTorqueErrors)");
937 rf.configure(argc,argv);
938
939 if (rf.check("help"))
940 {
941 printf ("\ncontrolBoardDumper usage:\n");
942 printf ("1) controlBoardDumper --robot icub --part left_arm --rate 10 --joints \"(0 1 2)\" \n");
943 printf (" All data from the controlBoarWrapper will be dumped, except for advanced debugInterface (default).\n");
944 printf ("\n2) controlBoardDumper --robot icub --part left_arm --rate 10 --joints \"(0 1 2)\" --dataToDump \"(xxx xxx)\"\n");
945 printf (" where xxx can be uset to select one (or more) from the following:\n");
946 printf (" getEncoders (joint position)\n");
947 printf (" getEncoderSpeeds (joint velocity)\n");
948 printf (" getEncoderAccelerations (joint acceleration)\n");
949 printf (" getPositionErrors (difference between desired and actual position)\n");
950 printf (" getTorqueErrors (difference between desired and measured torque, if available)\n");
951 printf (" getOutputs (Position Pid output)\n");
952 printf (" getCurrents (current given to the motor)\n");
953 printf (" getTorques (joint torques, if available)\n");
954 printf (" getPosPidReferences (last position referenece)\n");
955 printf (" getTrqPidReferences (last torque reference)\n");
956 printf (" getMotorEncoders (motor encoder position)\n");
957 printf (" getMotorEncoderSpeeds (motor encoder velocity)\n");
958 printf (" getMotorEncoderAccelerations (motor encoder acceleration\n");
959 printf (" getMotorsPwm (voltage (PWM) given to the motor)\n");
960 printf (" getControlModes (joint control mode)\n");
961 printf (" getInteractionModes (joint interaction mode)\n");
962 printf (" getTemperatures (motor temperatures)\n");
963 printf ("\n3) controlBoardDumper --robot icub --part left_arm --rate 10 --joints \"(0 1 2)\" --dataToDumpAll\n");
964 printf (" All data from the controlBoarWrapper will be dumped, including data from the debugInterface (getRotorxxx).\n");
965 printf ("\n --logToFile can be used to create log files storing the data\n\n");
966
967 return 0;
968 }
969
970 if (!yarp.checkNetwork())
971 {
972 yError()<<"YARP server not available!";
973 return 1;
974 }
975
976 DumpModule mod;
977 return mod.runModule(rf);
978}
979
980
virtual bool updateModule()
Definition main.cpp:893
DumpModule()
Definition main.cpp:434
virtual bool configure(ResourceFinder &rf)
Definition main.cpp:446
virtual bool close()
Definition main.cpp:898
void setInterface(IEncoders *)
void setInterface(IControlMode *, int joints)
void setInterface(IAmplifierControl *)
void setStamp(IPreciselyTimed *)
void setInterface(IEncoders *)
void setInterface(IInteractionMode *, int joints)
void setInterface(IMotorEncoders *)
void setInterface(IMotorEncoders *)
void setInterface(IAmplifierControl *)
void setInterface(IMotorEncoders *)
void setInterface(IPidControl *)
void setInterface(IPidControl *)
void setInterface(IPidControl *)
void setInterface(IEncoders *)
void setInterface(IMotor *)
void setInterface(IPidControl *)
void setInterface(ITorqueControl *)
void setInterface(ITorqueControl *)
void setThetaMap(int *, int)
void setGetter(GetData *)
void setDevice(PolyDriver *board_d, PolyDriver *debug_d, int rate, std::string portPrefix, std::string dataToDump, bool logOnDisk)
int n
int getDataToDump(ResourceFinder &rf, std::string *listOfData, int n, bool *needDebug)
Definition main.cpp:298
int getNumberConstraints(ResourceFinder &rf, int &n)
Definition main.cpp:261
#define NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP
Definition main.cpp:199
int getNumberDataToDump(ResourceFinder &rf, int &n)
Definition main.cpp:273
void getRate(ResourceFinder &rf, int &r)
Definition main.cpp:203
#define NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP
Definition main.cpp:198
int getNumberUsedJoints(ResourceFinder &rf, int &n)
Definition main.cpp:214
int getUsedJointsMap(ResourceFinder &rf, int n, int *thetaMap)
Definition main.cpp:233
int main()
Definition main.cpp:67
Copyright (C) 2008 RobotCub Consortium.