Fawkes API  Fawkes Development Version
message_handler_thread.cpp
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 }