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
message_handler_thread.cpp
1
2
/***************************************************************************
3
* message_handler_thread.cpp - OpenRAVE Thread
4
*
5
* Created: Fri Feb 25 15:08:00 2011
6
* Copyright 2011 Bahram Maleki-Fard
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 "message_handler_thread.h"
24
25
#include <interfaces/OpenRaveInterface.h>
26
27
using namespace
fawkes;
28
29
/** @class OpenRaveMessageHandlerThread "message_handler_thread.h"
30
* OpenRAVE Thread.
31
* This thread handles incoming messages for the OpenRaveInterface.
32
*
33
* @author Bahram Maleki-Fard
34
*/
35
36
/** Constructor.
37
* @param or_thread OpenRaveThread, main thread. */
38
OpenRaveMessageHandlerThread::OpenRaveMessageHandlerThread
(
OpenRaveThread
* or_thread)
39
:
Thread
(
"OpenRaveMessageHandlerThread"
,
Thread
::OPMODE_WAITFORWAKEUP),
40
BlockedTimingAspect
(
BlockedTimingAspect
::WAKEUP_HOOK_ACT),
41
__or_thread( or_thread ),
42
__if_openrave( 0 )
43
{
44
}
45
46
47
/** Destructor. */
48
OpenRaveMessageHandlerThread::~OpenRaveMessageHandlerThread
()
49
{
50
}
51
52
53
void
54
OpenRaveMessageHandlerThread::init
()
55
{
56
try
{
57
__if_openrave =
blackboard
->
open_for_writing
<
OpenRaveInterface
>(
"OpenRAVE"
);
58
}
catch
(
fawkes::Exception
&e) {
59
logger
->
log_warn
(
name
(),
"Could not open OpenRAVE interface for writing. Er:%s"
, e.
what
());
60
}
61
}
62
63
64
void
65
OpenRaveMessageHandlerThread::finalize
()
66
{
67
try
{
68
blackboard
->
close
(__if_openrave);
69
}
catch
(
fawkes::Exception
& e) {
70
logger
->
log_warn
(
name
(),
"Could not close OpenRAVE interface"
);
71
}
72
}
73
74
75
void
76
OpenRaveMessageHandlerThread::loop
()
77
{
78
while
( !__if_openrave->
msgq_empty
() ) {
79
__if_openrave->
set_success
(
false
);
80
__if_openrave->
set_final
(
false
);
81
Message
*m = __if_openrave->
msgq_first
(m);
82
__if_openrave->
set_msgid
(m->
id
());
83
__if_openrave->
write
();
84
85
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::AddObjectMessage
>()) {
86
OpenRaveInterface::AddObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
87
if
( __or_thread->
add_object
(msg->
name
(), msg->
path
()) )
88
{ __if_openrave->
set_success
(
true
); }
89
__if_openrave->
set_final
(
true
);
90
91
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::DeleteObjectMessage
>()) {
92
OpenRaveInterface::DeleteObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
93
if
( __or_thread->
delete_object
(msg->
name
()) )
94
{ __if_openrave->
set_success
(
true
); }
95
__if_openrave->
set_final
(
true
);
96
97
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::AttachObjectMessage
>()) {
98
OpenRaveInterface::AttachObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
99
if
( __or_thread->
attach_object
(msg->
name
()) )
100
{ __if_openrave->
set_success
(
true
); }
101
__if_openrave->
set_final
(
true
);
102
103
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::ReleaseObjectMessage
>()) {
104
OpenRaveInterface::ReleaseObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
105
if
( __or_thread->
release_object
(msg->
name
()) )
106
{ __if_openrave->
set_success
(
true
); }
107
__if_openrave->
set_final
(
true
);
108
109
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::ReleaseAllObjectsMessage
>()) {
110
OpenRaveInterface::ReleaseAllObjectsMessage
*msg = __if_openrave->
msgq_first
(msg);
111
if
( __or_thread->
release_all_objects
() )
112
{ __if_openrave->
set_success
(
true
); }
113
__if_openrave->
set_final
(
true
);
114
115
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::MoveObjectMessage
>()) {
116
OpenRaveInterface::MoveObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
117
if
( __or_thread->
move_object
(msg->
name
(), msg->
x
(), msg->
y
(), msg->
z
()) )
118
{ __if_openrave->
set_success
(
true
); }
119
__if_openrave->
set_final
(
true
);
120
121
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::RotateObjectQuatMessage
>()) {
122
OpenRaveInterface::RotateObjectQuatMessage
*msg = __if_openrave->
msgq_first
(msg);
123
if
( __or_thread->
rotate_object
(msg->
name
(), msg->
x
(), msg->
y
(), msg->
z
(), msg->
w
()) )
124
{ __if_openrave->
set_success
(
true
); }
125
__if_openrave->
set_final
(
true
);
126
127
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::RotateObjectMessage
>()) {
128
OpenRaveInterface::RotateObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
129
if
( __or_thread->
rotate_object
(msg->
name
(), msg->
x
(), msg->
y
(), msg->
z
()) )
130
{ __if_openrave->
set_success
(
true
); }
131
__if_openrave->
set_final
(
true
);
132
133
}
else
if
(__if_openrave->
msgq_first_is
<
OpenRaveInterface::RenameObjectMessage
>()) {
134
OpenRaveInterface::RenameObjectMessage
*msg = __if_openrave->
msgq_first
(msg);
135
if
( __or_thread->
rename_object
(msg->
name
(), msg->
newName
()) )
136
{ __if_openrave->
set_success
(
true
); }
137
__if_openrave->
set_final
(
true
);
138
139
}
else
{
140
logger
->
log_warn
(
name
(),
"Unknown message received"
);
141
}
142
143
__if_openrave->
msgq_pop
();
144
}
145
146
__if_openrave->
write
();
147
}
src
plugins
openrave
message_handler_thread.cpp
Generated by
1.8.1.2