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 }
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.