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
sensor_thread.cpp
1
2
/***************************************************************************
3
* sensor_thread.cpp - Laser thread that puses data into the interface
4
*
5
* Created: Wed Oct 08 13:32:57 2008
6
* Copyright 2006-2008 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 "sensor_thread.h"
24
#include "acquisition_thread.h"
25
26
#include <interfaces/Laser360Interface.h>
27
#include <interfaces/Laser720Interface.h>
28
29
using namespace
fawkes;
30
31
/** @class LaserSensorThread "sensor_thread.h"
32
* Laser sensor thread.
33
* This thread integrates into the Fawkes main loop at the sensor hook and
34
* publishes new data when available from the LaserAcquisitionThread.
35
* @author Tim Niemueller
36
*/
37
38
39
/** Constructor.
40
* @param cfg_name short name of configuration group
41
* @param cfg_prefix configuration path prefix
42
* @param aqt LaserAcquisitionThread to get data from
43
*/
44
LaserSensorThread::LaserSensorThread
(std::string &cfg_name,
45
std::string &cfg_prefix,
46
LaserAcquisitionThread
*aqt)
47
:
Thread
(
"LaserSensorThread"
,
Thread
::OPMODE_WAITFORWAKEUP),
48
BlockedTimingAspect
(
BlockedTimingAspect
::WAKEUP_HOOK_SENSOR_ACQUIRE)
49
{
50
set_name
(
"LaserSensorThread(%s)"
, cfg_name.c_str());
51
__aqt = aqt;
52
__cfg_name = cfg_name;
53
__cfg_prefix = cfg_prefix;
54
}
55
56
57
void
58
LaserSensorThread::init
()
59
{
60
__laser360_if = NULL;
61
__laser720_if = NULL;
62
63
bool
main_sensor =
false
;
64
65
__cfg_frame =
config
->
get_string
((__cfg_prefix +
"frame"
).c_str());
66
67
try
{
68
main_sensor =
config
->
get_bool
((__cfg_prefix +
"main_sensor"
).c_str());
69
}
catch
(
Exception
&e) {}
// ignored, assume no
70
71
__aqt->
pre_init
(
config
,
logger
);
72
73
__num_values = __aqt->
get_distance_data_size
();
74
75
std::string if_id = main_sensor ?
"Laser"
: (
"Laser "
+ __cfg_name);
76
77
if
(__num_values == 360) {
78
__laser360_if =
blackboard
->
open_for_writing
<
Laser360Interface
>(if_id.c_str());
79
__laser360_if->
set_frame
(__cfg_frame.c_str());
80
__laser360_if->
write
();
81
}
else
if
(__num_values == 720){
82
__laser720_if =
blackboard
->
open_for_writing
<
Laser720Interface
>(if_id.c_str());
83
__laser720_if->
set_frame
(__cfg_frame.c_str());
84
__laser720_if->
write
();
85
}
else
{
86
throw
Exception
(
"Laser acquisition thread must produce either 360 or 720 "
87
"distance values, but it produces %u"
, __aqt->
get_distance_data_size
());
88
}
89
90
}
91
92
93
void
94
LaserSensorThread::finalize
()
95
{
96
blackboard
->
close
(__laser360_if);
97
blackboard
->
close
(__laser720_if);
98
}
99
100
void
101
LaserSensorThread::loop
()
102
{
103
if
( __aqt->
lock_if_new_data
() ) {
104
if
(__num_values == 360) {
105
__laser360_if->
set_distances
(__aqt->
get_distance_data
());
106
__laser360_if->
write
();
107
}
else
if
(__num_values == 720) {
108
__laser720_if->
set_distances
(__aqt->
get_distance_data
());
109
__laser720_if->
write
();
110
}
111
__aqt->
unlock
();
112
}
113
}
src
plugins
laser
sensor_thread.cpp
Generated by
1.8.1.2