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 
47 /** Destructor. */
49 {
50 }
51 
52 
53 void
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
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
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 
86  logger->log_debug(name(), "StartViewer message received");
87  __or_thread->start_viewer();
88  __if_openrave->set_success(true);
89  __if_openrave->set_final(true);
90 
91  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::AddObjectMessage>()) {
92  OpenRaveInterface::AddObjectMessage *msg = __if_openrave->msgq_first(msg);
93  logger->log_debug(name(), "AddObject message received: name=%s, path=%s", msg->name(), msg->path());
94  if( __or_thread->add_object(msg->name(), msg->path()) )
95  { __if_openrave->set_success(true); }
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  __if_openrave->set_final(true);
104 
105  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::DeleteAllObjectsMessage>()) {
106  logger->log_debug(name(), "DeleteAllObjectsMessage received");
107  if( __or_thread->delete_all_objects() )
108  { __if_openrave->set_success(true); }
109  __if_openrave->set_final(true);
110 
111  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::AttachObjectMessage>()) {
112  OpenRaveInterface::AttachObjectMessage *msg = __if_openrave->msgq_first(msg);
113  logger->log_debug(name(), "AttachObjectMessage received: name=%s, manip_name=%s", msg->name(), msg->manip_name());
114  bool success = false;
115  if( strcmp(msg->manip_name(), "") == 0 ) {
116  success = __or_thread->attach_object(msg->name());
117  } else {
118  success = __or_thread->attach_object(msg->name(), msg->manip_name());
119  }
120  __if_openrave->set_success(success);
121  __if_openrave->set_final(true);
122 
123  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::ReleaseObjectMessage>()) {
124  OpenRaveInterface::ReleaseObjectMessage *msg = __if_openrave->msgq_first(msg);
125  logger->log_debug(name(), "ReleaseObjectMessage received: name=%s", msg->name());
126  if( __or_thread->release_object(msg->name()) )
127  { __if_openrave->set_success(true); }
128  __if_openrave->set_final(true);
129 
130  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::ReleaseAllObjectsMessage>()) {
131  OpenRaveInterface::ReleaseAllObjectsMessage *msg = __if_openrave->msgq_first(msg);
132  logger->log_debug(name(), "ReleaseAllObjectsMessage received");
133  if( __or_thread->release_all_objects() )
134  { __if_openrave->set_success(true); }
135  __if_openrave->set_final(true);
136 
137  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::MoveObjectMessage>()) {
138  OpenRaveInterface::MoveObjectMessage *msg = __if_openrave->msgq_first(msg);
139  logger->log_debug(name(), "MoveObjectMessage received: name=%s, x=%f, y=%f, z=%f",
140  msg->name(), msg->x(), msg->y(), msg->z());
141  if( __or_thread->move_object(msg->name(), msg->x(), msg->y(), msg->z()) )
142  { __if_openrave->set_success(true); }
143  __if_openrave->set_final(true);
144 
145  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::RotateObjectQuatMessage>()) {
146  OpenRaveInterface::RotateObjectQuatMessage *msg = __if_openrave->msgq_first(msg);
147  logger->log_debug(name(), "RotateObjectQuatMessage received: name=%s, x=%f, y=%f, z=%f, w=%f",
148  msg->name(), msg->x(), msg->y(), msg->z(), msg->w());
149  if( __or_thread->rotate_object(msg->name(), msg->x(), msg->y(), msg->z(), msg->w()) )
150  { __if_openrave->set_success(true); }
151  __if_openrave->set_final(true);
152 
153  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::RotateObjectMessage>()) {
154  OpenRaveInterface::RotateObjectMessage *msg = __if_openrave->msgq_first(msg);
155  logger->log_debug(name(), "RotateObjectMessage received: name=%s, x=%f, y=%f, z=%f",
156  msg->name(), msg->x(), msg->y(), msg->z());
157  if( __or_thread->rotate_object(msg->name(), msg->x(), msg->y(), msg->z()) )
158  { __if_openrave->set_success(true); }
159  __if_openrave->set_final(true);
160 
161  } else if (__if_openrave->msgq_first_is<OpenRaveInterface::RenameObjectMessage>()) {
162  OpenRaveInterface::RenameObjectMessage *msg = __if_openrave->msgq_first(msg);
163  logger->log_debug(name(), "RenameObjectMessage received: name=%s, new_name=%s", msg->name(), msg->newName());
164  if( __or_thread->rename_object(msg->name(), msg->newName()) )
165  { __if_openrave->set_success(true); }
166  __if_openrave->set_final(true);
167 
168  } else {
169  logger->log_warn(name(), "Unknown message received");
170  }
171 
172  __if_openrave->msgq_pop();
173  }
174 
175  __if_openrave->write();
176 }
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
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:197
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
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:661
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
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:44
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:1193
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Base class for exceptions in Fawkes.
Definition: exception.h:36
AttachObjectMessage Fawkes BlackBoard Interface Message.
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
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:95
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.