Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * message_handler_thread.cpp - OpenRAVE Thread 00004 * 00005 * Created: Fri Feb 25 15:08:00 2011 00006 * Copyright 2011 Bahram Maleki-Fard 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "message_handler_thread.h" 00024 00025 #include <interfaces/OpenRaveInterface.h> 00026 00027 using namespace fawkes; 00028 00029 /** @class OpenRaveMessageHandlerThread "message_handler_thread.h" 00030 * OpenRAVE Thread. 00031 * This thread handles incoming messages for the OpenRaveInterface. 00032 * 00033 * @author Bahram Maleki-Fard 00034 */ 00035 00036 /** Constructor. 00037 * @param or_thread OpenRaveThread, main thread. */ 00038 OpenRaveMessageHandlerThread::OpenRaveMessageHandlerThread(OpenRaveThread* or_thread) 00039 : Thread("OpenRaveMessageHandlerThread", Thread::OPMODE_WAITFORWAKEUP), 00040 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT), 00041 __or_thread( or_thread ), 00042 __if_openrave( 0 ) 00043 { 00044 } 00045 00046 00047 /** Destructor. */ 00048 OpenRaveMessageHandlerThread::~OpenRaveMessageHandlerThread() 00049 { 00050 } 00051 00052 00053 void 00054 OpenRaveMessageHandlerThread::init() 00055 { 00056 try { 00057 __if_openrave = blackboard->open_for_writing<OpenRaveInterface>("OpenRAVE"); 00058 } catch(fawkes::Exception &e) { 00059 logger->log_warn(name(), "Could not open OpenRAVE interface for writing. Er:%s", e.what()); 00060 } 00061 } 00062 00063 00064 void 00065 OpenRaveMessageHandlerThread::finalize() 00066 { 00067 try { 00068 blackboard->close(__if_openrave); 00069 } catch(fawkes::Exception& e) { 00070 logger->log_warn(name(), "Could not close OpenRAVE interface"); 00071 } 00072 } 00073 00074 00075 void 00076 OpenRaveMessageHandlerThread::loop() 00077 { 00078 while( !__if_openrave->msgq_empty() ) { 00079 __if_openrave->set_success(false); 00080 __if_openrave->set_final(false); 00081 Message *m = __if_openrave->msgq_first(m); 00082 __if_openrave->set_msgid(m->id()); 00083 __if_openrave->write(); 00084 00085 if (__if_openrave->msgq_first_is<OpenRaveInterface::AddObjectMessage>()) { 00086 OpenRaveInterface::AddObjectMessage *msg = __if_openrave->msgq_first(msg); 00087 if( __or_thread->add_object(msg->name(), msg->path()) ) 00088 { __if_openrave->set_success(true); } 00089 __if_openrave->set_final(true); 00090 00091 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::DeleteObjectMessage>()) { 00092 OpenRaveInterface::DeleteObjectMessage *msg = __if_openrave->msgq_first(msg); 00093 if( __or_thread->delete_object(msg->name()) ) 00094 { __if_openrave->set_success(true); } 00095 __if_openrave->set_final(true); 00096 00097 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::AttachObjectMessage>()) { 00098 OpenRaveInterface::AttachObjectMessage *msg = __if_openrave->msgq_first(msg); 00099 if( __or_thread->attach_object(msg->name()) ) 00100 { __if_openrave->set_success(true); } 00101 __if_openrave->set_final(true); 00102 00103 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::ReleaseObjectMessage>()) { 00104 OpenRaveInterface::ReleaseObjectMessage *msg = __if_openrave->msgq_first(msg); 00105 if( __or_thread->release_object(msg->name()) ) 00106 { __if_openrave->set_success(true); } 00107 __if_openrave->set_final(true); 00108 00109 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::ReleaseAllObjectsMessage>()) { 00110 OpenRaveInterface::ReleaseAllObjectsMessage *msg = __if_openrave->msgq_first(msg); 00111 if( __or_thread->release_all_objects() ) 00112 { __if_openrave->set_success(true); } 00113 __if_openrave->set_final(true); 00114 00115 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::MoveObjectMessage>()) { 00116 OpenRaveInterface::MoveObjectMessage *msg = __if_openrave->msgq_first(msg); 00117 if( __or_thread->move_object(msg->name(), msg->x(), msg->y(), msg->z()) ) 00118 { __if_openrave->set_success(true); } 00119 __if_openrave->set_final(true); 00120 00121 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::RotateObjectQuatMessage>()) { 00122 OpenRaveInterface::RotateObjectQuatMessage *msg = __if_openrave->msgq_first(msg); 00123 if( __or_thread->rotate_object(msg->name(), msg->x(), msg->y(), msg->z(), msg->w()) ) 00124 { __if_openrave->set_success(true); } 00125 __if_openrave->set_final(true); 00126 00127 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::RotateObjectMessage>()) { 00128 OpenRaveInterface::RotateObjectMessage *msg = __if_openrave->msgq_first(msg); 00129 if( __or_thread->rotate_object(msg->name(), msg->x(), msg->y(), msg->z()) ) 00130 { __if_openrave->set_success(true); } 00131 __if_openrave->set_final(true); 00132 00133 } else if (__if_openrave->msgq_first_is<OpenRaveInterface::RenameObjectMessage>()) { 00134 OpenRaveInterface::RenameObjectMessage *msg = __if_openrave->msgq_first(msg); 00135 if( __or_thread->rename_object(msg->name(), msg->newName()) ) 00136 { __if_openrave->set_success(true); } 00137 __if_openrave->set_final(true); 00138 00139 } else { 00140 logger->log_warn(name(), "Unknown message received"); 00141 } 00142 00143 __if_openrave->msgq_pop(); 00144 } 00145 00146 __if_openrave->write(); 00147 }