Fawkes API Fawkes Development Version
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
27using 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. */
39: Thread("OpenRaveMessageHandlerThread", Thread::OPMODE_WAITFORWAKEUP),
41 or_thread_(or_thread),
42 if_openrave_(0)
43{
44}
45
46/** Destructor. */
48{
49}
50
51void
53{
54 try {
55 if_openrave_ = blackboard->open_for_writing<OpenRaveInterface>("OpenRAVE");
56 } catch (fawkes::Exception &e) {
57 logger->log_warn(name(), "Could not open OpenRAVE interface for writing. Er:%s", e.what());
58 }
59}
60
61void
63{
64 try {
65 blackboard->close(if_openrave_);
66 } catch (fawkes::Exception &e) {
67 logger->log_warn(name(), "Could not close OpenRAVE interface");
68 }
69}
70
71void
73{
74 while (!if_openrave_->msgq_empty()) {
75 if_openrave_->set_success(false);
76 if_openrave_->set_final(false);
77 Message *m = if_openrave_->msgq_first(m);
78 if_openrave_->set_msgid(m->id());
79 if_openrave_->write();
80
82 logger->log_debug(name(), "StartViewer message received");
83 or_thread_->start_viewer();
84 if_openrave_->set_success(true);
85 if_openrave_->set_final(true);
86
87 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::AddObjectMessage>()) {
88 OpenRaveInterface::AddObjectMessage *msg = if_openrave_->msgq_first(msg);
90 "AddObject message received: name=%s, path=%s",
91 msg->name(),
92 msg->path());
93 if (or_thread_->add_object(msg->name(), msg->path())) {
94 if_openrave_->set_success(true);
95 }
96 if_openrave_->set_final(true);
97
98 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::DeleteObjectMessage>()) {
99 OpenRaveInterface::DeleteObjectMessage *msg = if_openrave_->msgq_first(msg);
100 logger->log_debug(name(), "DeleteObjectMessage received: name=%s", msg->name());
101 if (or_thread_->delete_object(msg->name())) {
102 if_openrave_->set_success(true);
103 }
104 if_openrave_->set_final(true);
105
106 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::DeleteAllObjectsMessage>()) {
107 logger->log_debug(name(), "DeleteAllObjectsMessage received");
108 if (or_thread_->delete_all_objects()) {
109 if_openrave_->set_success(true);
110 }
111 if_openrave_->set_final(true);
112
113 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::AttachObjectMessage>()) {
114 OpenRaveInterface::AttachObjectMessage *msg = if_openrave_->msgq_first(msg);
116 "AttachObjectMessage received: name=%s, manip_name=%s",
117 msg->name(),
118 msg->manip_name());
119 bool success = false;
120 if (strcmp(msg->manip_name(), "") == 0) {
121 success = or_thread_->attach_object(msg->name());
122 } else {
123 success = or_thread_->attach_object(msg->name(), msg->manip_name());
124 }
125 if_openrave_->set_success(success);
126 if_openrave_->set_final(true);
127
128 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::ReleaseObjectMessage>()) {
129 OpenRaveInterface::ReleaseObjectMessage *msg = if_openrave_->msgq_first(msg);
130 logger->log_debug(name(), "ReleaseObjectMessage received: name=%s", msg->name());
131 if (or_thread_->release_object(msg->name())) {
132 if_openrave_->set_success(true);
133 }
134 if_openrave_->set_final(true);
135
138 logger->log_debug(name(), "ReleaseAllObjectsMessage received");
139 if (or_thread_->release_all_objects()) {
140 if_openrave_->set_success(true);
141 }
142 if_openrave_->set_final(true);
143
144 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::MoveObjectMessage>()) {
145 OpenRaveInterface::MoveObjectMessage *msg = if_openrave_->msgq_first(msg);
147 "MoveObjectMessage received: name=%s, x=%f, y=%f, z=%f",
148 msg->name(),
149 msg->x(),
150 msg->y(),
151 msg->z());
152 if (or_thread_->move_object(msg->name(), msg->x(), msg->y(), msg->z())) {
153 if_openrave_->set_success(true);
154 }
155 if_openrave_->set_final(true);
156
157 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::RotateObjectQuatMessage>()) {
160 "RotateObjectQuatMessage received: name=%s, x=%f, y=%f, z=%f, w=%f",
161 msg->name(),
162 msg->x(),
163 msg->y(),
164 msg->z(),
165 msg->w());
166 if (or_thread_->rotate_object(msg->name(), msg->x(), msg->y(), msg->z(), msg->w())) {
167 if_openrave_->set_success(true);
168 }
169 if_openrave_->set_final(true);
170
171 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::RotateObjectMessage>()) {
172 OpenRaveInterface::RotateObjectMessage *msg = if_openrave_->msgq_first(msg);
174 "RotateObjectMessage received: name=%s, x=%f, y=%f, z=%f",
175 msg->name(),
176 msg->x(),
177 msg->y(),
178 msg->z());
179 if (or_thread_->rotate_object(msg->name(), msg->x(), msg->y(), msg->z())) {
180 if_openrave_->set_success(true);
181 }
182 if_openrave_->set_final(true);
183
184 } else if (if_openrave_->msgq_first_is<OpenRaveInterface::RenameObjectMessage>()) {
185 OpenRaveInterface::RenameObjectMessage *msg = if_openrave_->msgq_first(msg);
187 "RenameObjectMessage received: name=%s, new_name=%s",
188 msg->name(),
189 msg->newName());
190 if (or_thread_->rename_object(msg->name(), msg->newName())) {
191 if_openrave_->set_success(true);
192 }
193 if_openrave_->set_final(true);
194
195 } else {
196 logger->log_warn(name(), "Unknown message received");
197 }
198
199 if_openrave_->msgq_pop();
200 }
201
202 if_openrave_->write();
203}
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual ~OpenRaveMessageHandlerThread()
Destructor.
OpenRaveMessageHandlerThread(OpenRaveThread *or_thread)
Constructor.
virtual void init()
Initialize the thread.
OpenRAVE Thread.
virtual void start_viewer() const
Start Viewer.
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
virtual bool rotate_object(const std::string &name, float quat_x, float quat_y, float quat_z, float quat_w)
Rotate object by a quaternion.
virtual bool attach_object(const char *name, fawkes::OpenRaveRobotPtr &robot, const char *manip_name=NULL)
Attach a kinbody to the robot.
virtual bool release_all_objects(fawkes::OpenRaveRobotPtr &robot)
Release all grabbed kinbodys from the robot.
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z, fawkes::OpenRaveRobotPtr &robot)
Move object in the environment, relatively to robot.
virtual bool release_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot)
Release a kinbody from the robot.
virtual bool delete_all_objects()
Remove all objects from environment.
virtual bool delete_object(const std::string &name)
Remove object from environment.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
AddObjectMessage Fawkes BlackBoard Interface Message.
AttachObjectMessage Fawkes BlackBoard Interface Message.
char * manip_name() const
Get manip_name value.
DeleteAllObjectsMessage Fawkes BlackBoard Interface Message.
DeleteObjectMessage Fawkes BlackBoard Interface Message.
MoveObjectMessage Fawkes BlackBoard Interface Message.
ReleaseAllObjectsMessage Fawkes BlackBoard Interface Message.
ReleaseObjectMessage Fawkes BlackBoard Interface Message.
RenameObjectMessage Fawkes BlackBoard Interface Message.
RotateObjectMessage Fawkes BlackBoard Interface Message.
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
StartViewerMessage Fawkes BlackBoard Interface Message.
OpenRaveInterface Fawkes BlackBoard Interface.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_final(const bool new_final)
Set final value.
void set_success(const bool new_success)
Set success value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.