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
tf_example_thread.cpp
1
2
/***************************************************************************
3
* tf_example_thread.cpp - tf example thread
4
*
5
* Created: Tue Oct 25 18:01:36 2011
6
* Copyright 2011 Tim Niemueller [www.niemueller.de]
7
****************************************************************************/
8
9
/* This program is free software; you can redistribute it and/or modify
10
* it under the terms of the GNU General Public License as published by
11
* the Free Software Foundation; either version 2 of the License, or
12
* (at your option) any later version.
13
*
14
* This program is distributed in the hope that it will be useful,
15
* but WITHOUT ANY WARRANTY; without even the implied warranty of
16
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17
* GNU Library General Public License for more details.
18
*
19
* Read the full text in the LICENSE.GPL file in the doc directory.
20
*/
21
22
#include "tf_example_thread.h"
23
24
#include <tf/time_cache.h>
25
26
/** @class TfExampleThread "tf_example_thread.h"
27
* Main thread of tf example plugin.
28
* @author Tim Niemueller
29
*/
30
31
using namespace
fawkes;
32
33
/** Constructor. */
34
TfExampleThread::TfExampleThread
()
35
:
Thread
(
"TfExampleThread"
,
Thread
::OPMODE_WAITFORWAKEUP),
36
BlockedTimingAspect
(
BlockedTimingAspect
::WAKEUP_HOOK_THINK)
37
{
38
}
39
40
41
/** Destructor. */
42
TfExampleThread::~TfExampleThread
()
43
{
44
}
45
46
47
void
48
TfExampleThread::init
()
49
{
50
}
51
52
53
void
54
TfExampleThread::finalize
()
55
{
56
}
57
58
59
#define SOURCE "/rx28/tilt"
60
#define TARGET "/base_link"
61
62
void
63
TfExampleThread::loop
()
64
{
65
bool
world_frame_exists =
tf_listener
->
frame_exists
(SOURCE);
66
bool
robot_frame_exists =
tf_listener
->
frame_exists
(TARGET);
67
68
if
(! world_frame_exists || ! robot_frame_exists) {
69
logger
->
log_warn
(
name
(),
"Frame missing: %s %s %s %s"
,
70
SOURCE, world_frame_exists ?
"exists"
:
"missing"
,
71
TARGET, robot_frame_exists ?
"exists"
:
"missing"
);
72
}
else
{
73
tf::StampedTransform
transform;
74
try
{
75
tf_listener
->
lookup_transform
(TARGET, SOURCE, transform);
76
}
catch
(
tf::ExtrapolationException
&e) {
77
logger
->
log_debug
(
name
(),
"Extrapolation error"
);
78
return
;
79
}
catch
(
tf::ConnectivityException
&e) {
80
logger
->
log_debug
(
name
(),
"Connectivity exception: %s"
, e.
what
());
81
return
;
82
}
83
84
fawkes::Time
now;
85
double
diff;
86
if
(now >= transform.
stamp
) {
87
diff = now - &transform.
stamp
;
88
}
else
{
89
diff = transform.
stamp
- &now;
90
}
91
92
tf::Quaternion q = transform.getRotation();
93
tf::Vector3 v = transform.getOrigin();
94
95
const
tf::TimeCache
*world_cache =
tf_listener
->
get_frame_cache
(SOURCE);
96
const
tf::TimeCache
*robot_cache =
tf_listener
->
get_frame_cache
(TARGET);
97
98
logger
->
log_info
(
name
(),
"Transform %s -> %s, %f sec old: "
99
"T(%f,%f,%f) Q(%f,%f,%f,%f)"
,
100
transform.
frame_id
.c_str(), transform.
child_frame_id
.c_str(),
101
diff, v.x(), v.y(), v.z(), q.x(), q.y(), q.z(), q.w());
102
103
logger
->
log_info
(
name
(),
"World cache size: %zu Robot cache size: %zu"
,
104
world_cache->
get_list_length
(),
105
robot_cache->
get_list_length
());
106
107
}
108
}
src
plugins
examples
tf_example
tf_example_thread.cpp
Generated by
1.8.1.2