PluginROS.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Institute for Artificial Intelligence,
00005  *  Universität Bremen.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Institute for Artificial Intelligence,
00019  *     Universität Bremen, nor the names of its contributors may be
00020  *     used to endorse or promote products derived from this software
00021  *     without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *********************************************************************/
00036 
00040 #include <plugins/ros/PluginROS.h>
00041 
00042 
00043 namespace beliefstate {
00044   namespace plugins {
00045     PLUGIN_CLASS::PLUGIN_CLASS() {
00046       m_nhHandle = NULL;
00047       m_aspnAsyncSpinner = NULL;
00048       m_bRoslogMessages = false;
00049       m_bFirstContextReceived = false;
00050       m_bKeepSpinning = true;
00051       m_bSpinWorkerRunning = false;
00052       m_thrdSpinWorker = NULL;
00053       
00054       this->setPluginVersion("0.93");
00055     }
00056     
00057     PLUGIN_CLASS::~PLUGIN_CLASS() {
00058       this->shutdownSpinWorker();
00059       
00060       if(m_nhHandle) {
00061         delete m_nhHandle;
00062       }
00063       
00064       if(m_thrdSpinWorker) {
00065         delete m_thrdSpinWorker;
00066       }
00067     }
00068     
00069     Result PLUGIN_CLASS::init(int argc, char** argv) {
00070       Result resInit = defaultResult();
00071       
00072       CDesignator* cdConfig = this->getIndividualConfig();
00073       
00074       this->setSubscribedToEvent("symbolic-create-designator", true);
00075       this->setSubscribedToEvent("interactive-callback", true);
00076       this->setSubscribedToEvent("symbolic-add-image", true);
00077       
00078       if(cdConfig->floatValue("roslog-messages") != 0) {
00079         this->setSubscribedToEvent("status-message", false);
00080       }
00081       
00082       if(!ros::ok()) {
00083         std::string strROSNodeName = cdConfig->stringValue("node-name");
00084         
00085         if(strROSNodeName == "") {
00086           strROSNodeName = "beliefstate_ros";
00087         }
00088         
00089         this->info("Starting ROS node '" + strROSNodeName + "'.");
00090         
00091         ros::init(argc, argv, strROSNodeName);
00092         m_nhHandle = new ros::NodeHandle("~");
00093         
00094         if(ros::ok()) {
00095           m_srvCallback = m_nhHandle->advertiseService<PLUGIN_CLASS>("operate", &PLUGIN_CLASS::serviceCallback, this);
00096           
00097           m_pubLoggedDesignators = m_nhHandle->advertise<designator_integration_msgs::Designator>("/logged_designators", 1);
00098           m_pubInteractiveCallback = m_nhHandle->advertise<designator_integration_msgs::Designator>("/interactive_callback", 1);
00099           
00100           if(cdConfig->floatValue("roslog-messages") != 0) {
00101             std::string strTopic = cdConfig->stringValue("roslog-topic");
00102             
00103             if(strTopic != "") {
00104               m_pubStatusMessages = m_nhHandle->advertise<rosgraph_msgs::Log>(strTopic, 1);
00105               m_bRoslogMessages = true;
00106             } else {
00107               this->warn("You requested the status messages to be roslog'ged, but didn't specify a roslog topic. Ignoring the whole thing.");
00108             }
00109           }
00110           
00111           this->info("ROS node started. Starting to spin.");
00112           m_thrdSpinWorker = new boost::thread(&PLUGIN_CLASS::spinWorker, this);
00113         } else {
00114           resInit.bSuccess = false;
00115           resInit.strErrorMessage = "Failed to start ROS node.";
00116         }
00117       } else {
00118         this->warn("ROS node already started.");
00119       }
00120       
00121       return resInit;
00122     }
00123     
00124     Result PLUGIN_CLASS::deinit() {
00125       ros::shutdown();
00126       
00127       return defaultResult();
00128     }
00129     
00130     Result PLUGIN_CLASS::cycle() {
00131       Result resCycle = defaultResult();
00132       this->deployCycleData(resCycle);
00133       
00134       return resCycle;
00135     }
00136     
00137     void PLUGIN_CLASS::setKeepSpinning(bool bKeepSpinning) {
00138       m_mtxSpinWorker.lock();
00139       m_bKeepSpinning = bKeepSpinning;
00140       m_mtxSpinWorker.unlock();
00141     }
00142     
00143     bool PLUGIN_CLASS::keepSpinning() {
00144       m_mtxSpinWorker.lock();
00145       bool bReturn = m_bKeepSpinning;
00146       m_mtxSpinWorker.unlock();
00147       
00148       return bReturn;
00149     }
00150     
00151     void PLUGIN_CLASS::spinWorker() {
00152       ros::Rate rSpin(10000);
00153       
00154       this->setKeepSpinning(true);
00155       this->setSpinWorkerRunning(true);
00156       
00157       while(this->keepSpinning()) {
00158         ros::spinOnce();
00159         //rSpin.sleep();
00160       }
00161       
00162       this->setSpinWorkerRunning(false);
00163     }
00164     
00165     void PLUGIN_CLASS::setSpinWorkerRunning(bool bSpinWorkerRunning) {
00166       m_mtxSpinWorkerRunning.lock();
00167       m_bSpinWorkerRunning = bSpinWorkerRunning;
00168       m_mtxSpinWorkerRunning.unlock();
00169     }
00170     
00171     bool PLUGIN_CLASS::spinWorkerRunning() {
00172       m_mtxSpinWorkerRunning.lock();
00173       bool bReturn = m_bSpinWorkerRunning;
00174       m_mtxSpinWorkerRunning.unlock();
00175       
00176       return bReturn;
00177     }
00178     
00179     void PLUGIN_CLASS::shutdownSpinWorker() {
00180       this->setKeepSpinning(false);
00181       
00182       while(this->spinWorkerRunning()) {
00183         usleep(10000);
00184       }
00185       
00186       m_thrdSpinWorker->join();
00187     }
00188     
00189     bool PLUGIN_CLASS::serviceCallback(designator_integration_msgs::DesignatorCommunication::Request &req,
00190                                        designator_integration_msgs::DesignatorCommunication::Response &res) {
00191       bool bReturn = true;
00192       
00193       m_mtxGlobalInputLock.lock();
00194       
00195       // TODO(winkler): Maybe use this designator instance for the
00196       // events below, to not deserialize it twice.
00197       CDesignator* cdDesig = new CDesignator(req.request.designator);
00198       std::string strCBType = cdDesig->stringValue("_cb_type");
00199       delete cdDesig;
00200       
00201       transform(strCBType.begin(), strCBType.end(), strCBType.begin(), ::tolower);
00202       
00203       if(strCBType == "begin") {
00204         Event evBeginContext = defaultEvent("begin-context");
00205         evBeginContext.nContextID = createContextID();
00206         evBeginContext.cdDesignator = new CDesignator(req.request.designator);
00207         
00208         std::stringstream sts;
00209         sts << evBeginContext.nContextID;
00210         
00211         this->info("Beginning context (ID = " + sts.str() + "): '" + evBeginContext.cdDesignator->stringValue("_name") + "'");
00212         this->deployEvent(evBeginContext);
00213         
00214         CDesignator *desigResponse = new CDesignator();
00215         desigResponse->setType(ACTION);
00216         desigResponse->setValue(string("_id"), evBeginContext.nContextID);
00217         
00218         res.response.designators.push_back(desigResponse->serializeToMessage());
00219         delete desigResponse;
00220         
00221         if(!m_bFirstContextReceived) {
00222           this->info("First context received - logging is active.", true);
00223           m_bFirstContextReceived = true;
00224         }
00225       } else if(strCBType == "end") {
00226         Event evEndContext = defaultEvent("end-context");
00227         evEndContext.cdDesignator = new CDesignator(req.request.designator);
00228         
00229         int nContextID = (int)evEndContext.cdDesignator->floatValue("_id");
00230         std::stringstream sts;
00231         sts << nContextID;
00232         
00233         this->info("When ending context (ID = " + sts.str() + "), received " + this->getDesignatorTypeString(evEndContext.cdDesignator) + " designator");
00234         this->deployEvent(evEndContext);
00235         
00236         freeContextID(nContextID);
00237       } else if(strCBType == "alter") {
00238         Event evAlterContext = defaultEvent();
00239         evAlterContext.cdDesignator = new CDesignator(req.request.designator);
00240         
00241         if(evAlterContext.cdDesignator->stringValue("_type") == "service") {
00242           ServiceEvent seService = defaultServiceEvent();
00243           seService.smResultModifier = SM_IGNORE_RESULTS;
00244           
00245           seService.cdDesignator = evAlterContext.cdDesignator;
00246           seService.bPreserve = true;
00247           
00248           std::string strCommand = seService.cdDesignator->stringValue("command");
00249           transform(strCommand.begin(), strCommand.end(), strCommand.begin(), ::tolower);
00250           seService.strServiceName = strCommand;
00251           
00252           ServiceEvent seResult = this->deployServiceEvent(seService, true);
00253           
00254           this->waitForAssuranceToken(evAlterContext.cdDesignator->stringValue("_assurance_token"));
00255           
00256           if(seResult.cdDesignator) {
00257             res.response.designators.push_back(seResult.cdDesignator->serializeToMessage());
00258             
00259             if(seResult.bPreserve) {
00260               delete seResult.cdDesignator;
00261             }
00262           }
00263           
00264           delete seService.cdDesignator;
00265         } else if(evAlterContext.cdDesignator->stringValue("_type") == "alter") {
00266           this->info("When altering context, received " + this->getDesignatorTypeString(evAlterContext.cdDesignator) + " designator");
00267           
00268           std::string strCommand = evAlterContext.cdDesignator->stringValue("command");
00269           transform(strCommand.begin(), strCommand.end(), strCommand.begin(), ::tolower);
00270           
00271           if(strCommand == "add-image") {
00272             evAlterContext.strEventName = "add-image-from-topic";
00273             evAlterContext.nOpenRequestID = this->openNewRequestID();
00274           } else if(strCommand == "add-failure") {
00275             evAlterContext.strEventName = "add-failure";
00276           } else if(strCommand == "add-designator") {
00277             evAlterContext.strEventName = "add-designator";
00278           } else if(strCommand == "equate-designators") {
00279             evAlterContext.strEventName = "equate-designators";
00280           } else if(strCommand == "add-object") {
00281             evAlterContext.strEventName = "add-object";
00282           } else if(strCommand == "export-planlog") {
00283             evAlterContext.strEventName = "export-planlog";
00284           } else if(strCommand == "start-new-experiment") {
00285             evAlterContext.strEventName = "start-new-experiment";
00286           } else if(strCommand == "set-experiment-meta-data") {
00287             evAlterContext.strEventName = "set-experiment-meta-data";
00288           } else if(strCommand == "register-interactive-object") {
00289             evAlterContext.strEventName = "symbolic-add-object";
00290           } else if(strCommand == "unregister-interactive-object") {
00291             evAlterContext.strEventName = "symbolic-remove-object";
00292           } else if(strCommand == "set-interactive-object-menu") {
00293             evAlterContext.strEventName = "symbolic-set-interactive-object-menu";
00294           } else if(strCommand == "update-interactive-object-pose") {
00295             evAlterContext.strEventName = "symbolic-update-object-pose";
00296           } else if(strCommand == "catch-failure") {
00297             evAlterContext.strEventName = "catch-failure";
00298           } else if(strCommand == "rethrow-failure") {
00299             evAlterContext.strEventName = "rethrow-failure";
00300           } else {
00301             this->warn("Unknown command when altering context: '" + strCommand + "'");
00302           }
00303           
00304           this->waitForAssuranceToken(evAlterContext.cdDesignator->stringValue("_assurance_token"));
00305           
00306           this->deployEvent(evAlterContext, true);
00307         }
00308       } else {
00309         this->fail("Unknown callback operation: '" + strCBType + "'");
00310       }
00311       
00312       m_mtxGlobalInputLock.unlock();
00313       
00314       return bReturn;
00315     }
00316     
00317     void PLUGIN_CLASS::consumeEvent(Event evEvent) {
00318       if(evEvent.bRequest == false) {
00319         this->closeRequestID(evEvent.nOpenRequestID);
00320       } else {
00321         if(evEvent.strEventName == "symbolic-add-image") {
00322           this->closeRequestID(evEvent.nOpenRequestID);
00323         } else if(evEvent.strEventName == "symbolic-create-designator") {
00324           if(evEvent.cdDesignator) {
00325             m_pubLoggedDesignators.publish(evEvent.cdDesignator->serializeToMessage());
00326           }
00327         } else if(evEvent.strEventName == "interactive-callback") {
00328           if(evEvent.cdDesignator) {
00329             m_pubInteractiveCallback.publish(evEvent.cdDesignator->serializeToMessage());
00330           }
00331         } else if(evEvent.strEventName == "status-message") {
00332           if(m_bRoslogMessages) {
00333             StatusMessage msgStatus = evEvent.msgStatusMessage;
00334             rosgraph_msgs::Log rgmLogMessage;
00335             
00336             if(msgStatus.bBold == true) {
00337               rgmLogMessage.level = rosgraph_msgs::Log::WARN;
00338             } else {
00339               rgmLogMessage.level = rosgraph_msgs::Log::INFO;
00340             }
00341             
00342             rgmLogMessage.name = msgStatus.strPrefix;
00343             rgmLogMessage.msg = msgStatus.strMessage;
00344             
00345             m_pubStatusMessages.publish(rgmLogMessage);
00346           }
00347         }
00348       }
00349     }
00350     
00351     Event PLUGIN_CLASS::consumeServiceEvent(ServiceEvent seServiceEvent) {
00352       Event evReturn = Plugin::consumeServiceEvent(seServiceEvent);
00353       
00354       //this->info("Consume service event of type '" + seServiceEvent.strServiceName + "'!");
00355       
00356       return evReturn;
00357     }
00358     
00359     string PLUGIN_CLASS::getDesignatorTypeString(CDesignator* desigDesignator) {
00360       std::string strDesigType = "UNKNOWN";
00361       
00362       switch(desigDesignator->type()) {
00363       case ACTION:
00364         strDesigType = "ACTION";
00365         break;
00366         
00367       case OBJECT:
00368         strDesigType = "OBJECT";
00369         break;
00370         
00371       case LOCATION:
00372         strDesigType = "LOCATION";
00373         break;
00374         
00375       default:
00376         break;
00377       }
00378       
00379       return strDesigType;
00380     }
00381     
00382     void PLUGIN_CLASS::waitForAssuranceToken(std::string strToken) {
00383       if(strToken != "") {
00384         // Wait for issueance of assurance token.
00385         if(waitForGlobalToken(strToken, 2.0)) { // Timeout of 2.0 seconds
00386           revokeGlobalToken(strToken);
00387         } else {
00388           this->warn("Failed to deliver assurance token '" + strToken + "'. Waiting for it was a waste of time. Check your model, something's off.");
00389         }
00390       }
00391           
00392     }
00393   }
00394   
00395   extern "C" plugins::PLUGIN_CLASS* createInstance() {
00396     return new plugins::PLUGIN_CLASS();
00397   }
00398   
00399   extern "C" void destroyInstance(plugins::PLUGIN_CLASS* icDestroy) {
00400     delete icDestroy;
00401   }
00402 }


beliefstate
Author(s): Jan Winkler
autogenerated on Sun Oct 5 2014 22:30:15