Fawkes API
Fawkes Development Version
Main Page
Related Pages
Modules
Namespaces
Classes
Files
File List
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
*/
40
KatanaGripperThread::KatanaGripperThread
(
fawkes::RefPtr<fawkes::KatanaController>
katana,
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
54
KatanaGripperThread::set_mode
(
gripper_mode_t
mode)
55
{
56
__mode = mode;
57
}
58
59
60
void
61
KatanaGripperThread::once
()
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
;
74
_error_code
=
fawkes::KatanaInterface::ERROR_CMD_START_FAILED
;
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
{
84
_katana
->
read_sensor_data
();
85
_katana
->
read_motor_data
();
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"
);
92
_error_code
=
fawkes::KatanaInterface::ERROR_COMMUNICATION
;
93
break
;
94
}
95
}
96
97
try
{
98
final
=
_katana
->
final
();
99
}
catch
(
fawkes::KatanaMotorCrashedException
&e) {
100
_logger
->
log_warn
(
"KatanaMotorControlTrhead"
, e.
what
());
101
_error_code
=
fawkes::KatanaInterface::ERROR_MOTOR_CRASHED
;
102
break
;
103
}
104
}
105
106
_logger
->
log_debug
(
"KatanaGripperThread"
,
"Gripper motion finished"
);
107
108
_finished
=
true
;
109
}
src
plugins
katana
gripper_thread.cpp
Generated by
1.8.3.1