Fawkes API  Fawkes Development Version
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  * 2012-2014 Bahram Maleki-Fard
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "gripper_thread.h"
25 #include "controller.h"
26 #include "exception.h"
27 
28 #include <interfaces/KatanaInterface.h>
29 
30 #include <unistd.h>
31 
32 /** @class KatanaGripperThread "gripper_thread.h"
33  * Katana gripper thread.
34  * This thread opens or closes the gripper when started.
35  * @author Tim Niemueller
36  */
37 
38 /** Constructor.
39  * @param katana katana controller base class
40  * @param logger logger
41  * @param poll_interval_ms interval in ms between two checks if the
42  * final position has been reached
43  */
45  fawkes::Logger *logger,
46  unsigned int poll_interval_ms)
47  : KatanaMotionThread("KatanaGripperThread", katana, logger)
48 {
49  __mode = OPEN_GRIPPER;
50  __poll_interval_usec = poll_interval_ms * 1000;
51 }
52 
53 
54 /** Set mode.
55  * @param mode open, either open or close
56  */
57 void
59 {
60  __mode = mode;
61 }
62 
63 
64 void
66 {
67  try {
68  // non-blocking call
69  if (__mode == CLOSE_GRIPPER) {
70  _katana->gripper_close(/* wait */ false);
71  } else {
72  _katana->gripper_open(/* wait */ false);
73  }
74 
75  } catch (fawkes::Exception &e) {
76  _logger->log_warn("KatanaGripperThread", "Starting gripper motion failed (ignoring): %s", e.what());
77  _finished = true;
79  return;
80  }
81 
82  // check if final
83  bool final = false;
84  short num_errors = 0;
85  while ( !final ) {
86  usleep(__poll_interval_usec);
87  try {
90  } catch (fawkes::Exception &e) {
91  if (++num_errors <= 10) {
92  _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
93  continue;
94  } else {
95  _logger->log_warn("KatanaMotorControlThread", "Receiving sensor/motor data failed too often, aborting");
97  break;
98  }
99  }
100 
101  try {
102  final = _katana->final();
104  _logger->log_warn("KatanaMotorControlTrhead", e.what());
106  break;
107  }
108  }
109 
110  _logger->log_debug("KatanaGripperThread", "Gripper motion finished");
111 
112  _finished = true;
113 }
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void gripper_close(bool blocking=false)=0
Close Gripper.
KatanaGripperThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:48
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:52
Katana motion thread base class.
Definition: motion_thread.h:35
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
Base class for exceptions in Fawkes.
Definition: exception.h:36
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_mode(gripper_mode_t mode)
Set mode.
gripper_mode_t
Gripper execution mode.
At least one motor crashed.
Definition: exception.h:45
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:50
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:54
virtual void once()
Execute an action exactly once.
virtual void gripper_open(bool blocking=false)=0
Open Gripper.
virtual bool final()=0
Check if movement is final.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
Interface for logging.
Definition: logger.h:34