Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
gripper_thread.cpp
1 
2 /***************************************************************************
3  * gripper_thread.cpp - Katana gripper one-time thread
4  *
5  * Created: Thu Jun 11 11:59:38 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "gripper_thread.h"
24 #include "controller.h"
25 #include "exception.h"
26 
27 
28 /** @class KatanaGripperThread "gripper_thread.h"
29  * Katana gripper thread.
30  * This thread opens or closes the gripper when started.
31  * @author Tim Niemueller
32  */
33 
34 /** Constructor.
35  * @param katana katana controller base class
36  * @param logger logger
37  * @param poll_interval_ms interval in ms between two checks if the
38  * final position has been reached
39  */
41  fawkes::Logger *logger,
42  unsigned int poll_interval_ms)
43  : KatanaMotionThread("KatanaGripperThread", katana, logger)
44 {
45  __mode = OPEN_GRIPPER;
46  __poll_interval_usec = poll_interval_ms * 1000;
47 }
48 
49 
50 /** Set mode.
51  * @param mode open, either open or close
52  */
53 void
55 {
56  __mode = mode;
57 }
58 
59 
60 void
62 {
63  try {
64  // non-blocking call
65  if (__mode == CLOSE_GRIPPER) {
66  _katana->gripper_close(/* wait */ false);
67  } else {
68  _katana->gripper_open(/* wait */ false);
69  }
70 
71  } catch (fawkes::Exception &e) {
72  _logger->log_warn("KatanaGripperThread", "Starting gripper motion failed (ignoring): %s", e.what());
73  _finished = true;
75  return;
76  }
77 
78  // check if final
79  bool final = false;
80  short num_errors = 0;
81  while ( !final ) {
82  usleep(__poll_interval_usec);
83  try {
86  } catch (fawkes::Exception &e) {
87  if (++num_errors <= 10) {
88  _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
89  continue;
90  } else {
91  _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting");
93  break;
94  }
95  }
96 
97  try {
98  final = _katana->final();
100  _logger->log_warn("KatanaMotorControlTrhead", e.what());
102  break;
103  }
104  }
105 
106  _logger->log_debug("KatanaGripperThread", "Gripper motion finished");
107 
108  _finished = true;
109 }