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 
26 #include "controller.h"
27 #include "exception.h"
28 
29 #include <interfaces/KatanaInterface.h>
30 
31 #include <unistd.h>
32 
33 /** @class KatanaGripperThread "gripper_thread.h"
34  * Katana gripper thread.
35  * This thread opens or closes the gripper when started.
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor.
40  * @param katana katana controller base class
41  * @param logger logger
42  * @param poll_interval_ms interval in ms between two checks if the
43  * final position has been reached
44  */
46  fawkes::Logger * logger,
47  unsigned int poll_interval_ms)
48 : KatanaMotionThread("KatanaGripperThread", katana, logger)
49 {
50  mode_ = OPEN_GRIPPER;
51  poll_interval_usec_ = poll_interval_ms * 1000;
52 }
53 
54 /** Set mode.
55  * @param mode open, either open or close
56  */
57 void
59 {
60  mode_ = mode;
61 }
62 
63 void
65 {
66  try {
67  // non-blocking call
68  if (mode_ == CLOSE_GRIPPER) {
69  _katana->gripper_close(/* wait */ false);
70  } else {
71  _katana->gripper_open(/* wait */ false);
72  }
73 
74  } catch (fawkes::Exception &e) {
75  _logger->log_warn("KatanaGripperThread",
76  "Starting gripper motion failed (ignoring): %s",
77  e.what());
78  _finished = true;
80  return;
81  }
82 
83  // check if final
84  bool final = false;
85  short num_errors = 0;
86  while (!final) {
87  usleep(poll_interval_usec_);
88  try {
91  } catch (fawkes::Exception &e) {
92  if (++num_errors <= 10) {
93  _logger->log_warn("KatanaMotorControlThread", "Reading sensor/motor data failed, retrying");
94  continue;
95  } else {
96  _logger->log_warn("KatanaMotorControlThread",
97  "Receiving sensor/motor data failed too often, aborting");
99  break;
100  }
101  }
102 
103  try {
104  final = _katana->final();
106  _logger->log_warn("KatanaMotorControlTrhead", "Motor crashed: %s", e.what());
108  break;
109  }
110  }
111 
112  _logger->log_debug("KatanaGripperThread", "Gripper motion finished");
113 
114  _finished = true;
115 }
void set_mode(gripper_mode_t mode)
Set mode.
gripper_mode_t
Gripper execution mode.
@ CLOSE_GRIPPER
Close gripper.
@ OPEN_GRIPPER
Open gripper.
virtual void once()
Execute an action exactly once.
KatanaGripperThread(fawkes::RefPtr< fawkes::KatanaController > katana, fawkes::Logger *logger, unsigned int poll_interval_ms)
Constructor.
Katana motion thread base class.
Definition: motion_thread.h:36
fawkes::Logger * _logger
Logger.
Definition: motion_thread.h:52
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
Definition: motion_thread.h:48
bool _finished
Set to true when motion is finished, to false on reset.
Definition: motion_thread.h:50
unsigned int _error_code
Set to the desired error code on error.
Definition: motion_thread.h:54
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual void gripper_open(bool blocking=false)=0
Open Gripper.
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.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual bool final()=0
Check if movement is final.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
At least one motor crashed.
Definition: exception.h:44
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.