GoThrough.cpp
Go to the documentation of this file.
00001 #include "GoThrough.h"
00002 
00003 #include "ActionFactory.h"
00004 
00005 #include "bwi_kr_execution/CurrentStateQuery.h"
00006 
00007 #include <ros/console.h>
00008 #include <ros/ros.h>
00009 
00010 #include <algorithm>
00011 
00012 using namespace std;
00013 
00014 namespace bwi_krexec {
00015   
00016   static vector<string> createVector(const std::string& doorName) {
00017     vector<string> paramVector(1);
00018     paramVector[0] = doorName;
00019     
00020     return paramVector;
00021   }
00022     
00023   
00024 GoThrough::GoThrough(const std::string& doorName): 
00025               LogicalNavigation("gothrough",createVector(doorName)),
00026               failed(false){}
00027  
00028  
00029  struct IsFluentAt {
00030    
00031    bool operator()(const bwi_kr_execution::AspFluent& fluent) {
00032      return fluent.name == "at";
00033    }
00034    
00035  };
00036  
00037 void GoThrough::run() {
00038   
00039   ros::NodeHandle n;
00040   ros::ServiceClient krClient = n.serviceClient<bwi_kr_execution::CurrentStateQuery> ( "current_state_query" );
00041   krClient.waitForExistence();
00042   
00043   bwi_kr_execution::CurrentStateQuery csq;
00044   
00045   krClient.call(csq);
00046   
00047   vector<bwi_kr_execution::AspFluent>::const_iterator atIt = 
00048                     find_if(csq.response.answer.fluents.begin(), csq.response.answer.fluents.end(), IsFluentAt());
00049   
00050    bool error = false;
00051    string initialPosition = "";
00052                     
00053   if(atIt == csq.response.answer.fluents.end()) {
00054     ROS_ERROR("ApproachDoor: fluent \"at\" missing ");
00055     error = true;
00056   }
00057   else 
00058     initialPosition = atIt->variables[0];
00059   
00060   bwi_krexec::LogicalNavigation::run();
00061   
00062    krClient.call(csq);
00063   
00064   atIt = find_if(csq.response.answer.fluents.begin(), csq.response.answer.fluents.end(), IsFluentAt());
00065                     
00066   if(!error && atIt != csq.response.answer.fluents.end()) 
00067     failed = initialPosition == atIt->variables[0];
00068   
00069 }
00070 
00071 static ActionFactory gothroughFactory(new GoThrough(""));
00072   
00073   
00074 }


bwi_kr_execution
Author(s): Matteo Leonetti, Piyush Khandelwal
autogenerated on Fri Aug 28 2015 10:14:46