Fawkes API  Fawkes Development Version
 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. */
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 
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 }
RotateObjectQuatMessage Fawkes BlackBoard Interface Message.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:43
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:972
void set_success(const bool new_success)
Set success value.
ReleaseAllObjectsMessage Fawkes BlackBoard Interface Message.
virtual bool attach_object(const std::string &name, fawkes::OpenRaveRobot *robot=NULL)
Attach a kinbody to the robot.
void set_final(const bool new_final)
Set final value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:495
unsigned int id() const
Get message ID.
Definition: message.cpp:197
virtual bool move_object(const std::string &name, float trans_x, float trans_y, float trans_z, fawkes::OpenRaveRobot *robot=NULL)
Move object in the environment.
virtual ~OpenRaveMessageHandlerThread()
Destructor.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
RotateObjectMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
virtual bool delete_object(const std::string &name)
Remove object from environment.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1118
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
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:1104
char * newName() const
Get newName value.
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.
virtual bool release_object(const std::string &name, fawkes::OpenRaveRobot *robot=NULL)
Release a kinbody from the robot.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:305
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
ReleaseObjectMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:95
RenameObjectMessage Fawkes BlackBoard Interface Message.
virtual void init()
Initialize the thread.
virtual bool rename_object(const std::string &name, const std::string &new_name)
Rename object.
OpenRAVE Thread.
DeleteObjectMessage Fawkes BlackBoard Interface Message.
OpenRaveMessageHandlerThread(OpenRaveThread *or_thread)
Constructor.
virtual bool add_object(const std::string &name, const std::string &filename)
Add an object to the environment.
MoveObjectMessage Fawkes BlackBoard Interface Message.
virtual bool release_all_objects(fawkes::OpenRaveRobot *robot=NULL)
Release all grabbed kinbodys from the robot.
OpenRaveInterface Fawkes BlackBoard Interface.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:43
virtual void close(Interface *interface)=0
Close interface.
AddObjectMessage Fawkes BlackBoard Interface Message.