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
goto_thread.cpp
1
2
/***************************************************************************
3
* goto_thread.cpp - Katana goto one-time thread
4
*
5
* Created: Wed Jun 10 11:45:31 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 "goto_thread.h"
24
#include "controller.h"
25
#include "exception.h"
26
27
#include <cstdlib>
28
29
/** @class KatanaGotoThread "goto_thread.h"
30
* Katana linear goto thread.
31
* This thread moves the arm into a specified position.
32
* @author Tim Niemueller
33
*/
34
35
/** Constructor.
36
* @param katana katana controller base class
37
* @param logger logger
38
* @param poll_interval_ms interval in ms between two checks if the
39
* final position has been reached
40
*/
41
KatanaGotoThread::KatanaGotoThread
(
fawkes::RefPtr<fawkes::KatanaController>
katana,
42
fawkes::Logger
*logger,
43
unsigned
int
poll_interval_ms)
44
:
KatanaMotionThread
(
"KatanaGotoThread"
, katana, logger)
45
{
46
__poll_interval_usec = poll_interval_ms * 1000;
47
}
48
49
50
/** Set target position.
51
* @param x X coordinate relative to base
52
* @param y Y coordinate relative to base
53
* @param z Z coordinate relative to base
54
* @param phi Phi Euler angle of tool
55
* @param theta Theta Euler angle of tool
56
* @param psi Psi Euler angle of tool
57
*/
58
void
59
KatanaGotoThread::set_target
(
float
x,
float
y,
float
z,
60
float
phi,
float
theta,
float
psi)
61
{
62
__x = x;
63
__y = y;
64
__z = z;
65
__phi = phi;
66
__theta = theta;
67
__psi = psi;
68
}
69
70
void
71
KatanaGotoThread::once
()
72
{
73
try
{
74
// non-blocking call
75
_katana
->
move_to
(__x, __y, __z, __phi, __theta, __psi);
76
}
catch
(
fawkes::KatanaNoSolutionException
&e) {
77
_logger
->
log_warn
(
"KatanaGotoThread"
,
"Initiating goto failed (no solution, ignoring): %s"
, e.
what
());
78
_finished
=
true
;
79
_error_code
=
fawkes::KatanaInterface::ERROR_NO_SOLUTION
;
80
return
;
81
}
catch
(
fawkes::Exception
&e) {
82
_logger
->
log_warn
(
"KatanaGotoThread"
,
"Initiating goto failed (ignoring): %s"
, e.
what
());
83
_finished
=
true
;
84
_error_code
=
fawkes::KatanaInterface::ERROR_CMD_START_FAILED
;
85
return
;
86
}
87
88
// check if final
89
bool
final
=
false
;
90
short
num_errors = 0;
91
while
( !
final
) {
92
usleep(__poll_interval_usec);
93
try
{
94
_katana
->
read_sensor_data
();
95
_katana
->
read_motor_data
();
96
}
catch
(
fawkes::Exception
&e) {
97
if
(++num_errors <= 10) {
98
_logger
->
log_warn
(
"KatanaMotorControlThread"
,
"Reading sensor/motor data failed, retrying"
);
99
continue
;
100
}
else
{
101
_logger
->
log_warn
(
"KatanaMotorControlThread"
,
"Receiving sensor/motor data failed too often, aborting"
);
102
_error_code
=
fawkes::KatanaInterface::ERROR_COMMUNICATION
;
103
break
;
104
}
105
}
106
107
try
{
108
final
=
_katana
->
final
();
109
}
catch
(
fawkes::KatanaMotorCrashedException
&e) {
110
_logger
->
log_warn
(
"KatanaMotorControlTrhead"
, e.
what
());
111
_error_code
=
fawkes::KatanaInterface::ERROR_MOTOR_CRASHED
;
112
break
;
113
}
114
}
115
116
_logger
->
log_debug
(
name
(),
"Position (%f,%f,%f, %f,%f,%f) reached"
,
117
__x, __y, __z, __phi, __theta, __psi);
118
119
_finished
=
true
;
120
}
src
plugins
katana
goto_thread.cpp
Generated by
1.8.1.1