GotoObject.cpp
Go to the documentation of this file.
00001 #include "GotoObject.h"
00002 
00003 #include "ActionFactory.h"
00004 #include "../msgs_utils.h"
00005 
00006 #include "actasp/AnswerSet.h"
00007 
00008 #include "bwi_kr_execution/CurrentStateQuery.h"
00009 
00010 #include <ros/ros.h>
00011 
00012 #include <algorithm>
00013 
00014 
00015 
00016 
00017 using namespace std;
00018 using namespace actasp;
00019 
00020 namespace bwi_krexec {
00021   
00022   static vector<string> createVector(const std::string& objectName) {
00023     vector<string> paramVector(1);
00024     paramVector[0] = objectName;
00025     
00026     return paramVector;
00027   }
00028     
00029   
00030 GotoObject::GotoObject(const std::string& objectName): 
00031               LogicalNavigation("goto",createVector(objectName)),
00032               failed(false){}
00033  
00034  
00035  struct IsFluentAt {
00036    
00037    bool operator()(const bwi_kr_execution::AspFluent& fluent) {
00038      return fluent.name == "at";
00039    }
00040    
00041  };
00042  
00043 void GotoObject::run()  {
00044   
00045   bwi_krexec::LogicalNavigation::run();
00046 
00047   ros::NodeHandle n;
00048   ros::ServiceClient krClient = n.serviceClient<bwi_kr_execution::CurrentStateQuery> ( "current_state_query" );
00049   krClient.waitForExistence();
00050   
00051   bwi_kr_execution::CurrentStateQuery csq;
00052   
00053   krClient.call(csq);
00054   
00055   AnswerSet answer = TranslateAnswerSet()(csq.response.answer);
00056   
00057   failed = !answer.contains(AspFluent("facing",this->getParameters(),0));
00058   
00059   
00060 }
00061 
00062 static ActionFactory gotoFactory(new GotoObject(""));
00063   
00064   
00065 }


bwi_kr_execution
Author(s): Matteo Leonetti, Piyush Khandelwal
autogenerated on Thu Jun 6 2019 17:57:37