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 
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. */
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 /** Destructor. */
48 {
49 }
50 
51 void
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 
61 void
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 
71 void
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);
115  logger->log_debug(name(),
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 
136  } else if (if_openrave_->msgq_first_is<OpenRaveInterface::ReleaseAllObjectsMessage>()) {
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);
146  logger->log_debug(name(),
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>()) {
158  OpenRaveInterface::RotateObjectQuatMessage *msg = if_openrave_->msgq_first(msg);
159  logger->log_debug(name(),
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);
173  logger->log_debug(name(),
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);
186  logger->log_debug(name(),
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 }
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:190
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1026
void set_success(const bool new_success)
Set success value.
ReleaseAllObjectsMessage Fawkes BlackBoard Interface Message.
Fawkes library namespace.
void set_final(const bool new_final)
Set final value.
virtual bool attach_object(const char *name, fawkes::OpenRaveRobotPtr &robot, const char *manip_name=NULL)
Attach a kinbody to the robot.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
Thread class encapsulation of pthreads.
Definition: thread.h:45
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
virtual bool delete_all_objects()
Remove all objects from environment.
virtual ~OpenRaveMessageHandlerThread()
Destructor.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual bool release_object(const std::string &name, fawkes::OpenRaveRobotPtr &robot)
Release a kinbody from the robot.
RotateObjectMessage Fawkes BlackBoard Interface Message.
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.
Thread aspect to use blocked timing.
virtual bool delete_object(const std::string &name)
Remove object from environment.
StartViewerMessage Fawkes BlackBoard Interface Message.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1179
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Base class for exceptions in Fawkes.
Definition: exception.h:35
AttachObjectMessage Fawkes BlackBoard Interface Message.
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
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.
DeleteAllObjectsMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:100
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ReleaseObjectMessage Fawkes BlackBoard Interface Message.
virtual bool release_all_objects(fawkes::OpenRaveRobotPtr &robot)
Release all grabbed kinbodys from the robot.
RenameObjectMessage Fawkes BlackBoard Interface Message.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void init()
Initialize the thread.
virtual void start_viewer() const
Start Viewer.
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
char * manip_name() const
Get manip_name value.
OpenRAVE Thread.
DeleteObjectMessage Fawkes BlackBoard Interface Message.
OpenRaveMessageHandlerThread(OpenRaveThread *or_thread)
Constructor.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
MoveObjectMessage Fawkes BlackBoard Interface Message.
OpenRaveInterface Fawkes BlackBoard Interface.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.
AddObjectMessage Fawkes BlackBoard Interface Message.