OpenDoor.cpp
Go to the documentation of this file.
00001 #include "OpenDoor.h"
00002 
00003 
00004 #include "CallGUI.h"
00005 #include "LogicalNavigation.h"
00006 
00007 #include "ActionFactory.h"
00008 
00009 #include "actasp/AspFluent.h"
00010 
00011 #include "bwi_kr_execution/CurrentStateQuery.h"
00012 #include "bwi_kr_execution/AspRule.h"
00013 #include "bwi_kr_execution/AspFluent.h"
00014 
00015 #include <ros/ros.h>
00016 
00017 using namespace std;
00018 
00019 namespace bwi_krexec {
00020 
00021 OpenDoor::OpenDoor() : 
00022             door(),
00023             done(false),
00024             asked(false),
00025             open(false),
00026             failed(false),
00027             startTime(){}
00028 
00029   
00030 void OpenDoor::run() {
00031   if(!asked) {
00032     CallGUI askToOpen("askToOpen", CallGUI::DISPLAY,  "Can you open door " + door + ", please?");
00033     askToOpen.run();
00034     asked = true;
00035     startTime = ros::Time::now();
00036   }
00037   
00038   if(!open) {
00039     vector<string> params;
00040     params.push_back(door);
00041     LogicalNavigation senseDoor("sensedoor",params);
00042    
00043     senseDoor.run();
00044     
00045     //check if door is open
00046     ros::NodeHandle n;
00047     ros::ServiceClient currentClient = n.serviceClient<bwi_kr_execution::CurrentStateQuery> ( "current_state_query" );
00048     
00049     bwi_kr_execution::AspFluent openFluent;
00050     openFluent.name = "open";
00051     openFluent.timeStep = 0;
00052     openFluent.variables.push_back(door);
00053     
00054     bwi_kr_execution::AspRule rule;
00055     rule.head.push_back(openFluent);
00056     
00057     bwi_kr_execution::CurrentStateQuery csq;
00058     csq.request.query.push_back(rule);
00059     
00060     currentClient.call(csq);
00061     
00062     open = csq.response.answer.satisfied;
00063     
00064     if(!open && (ros::Time::now() - startTime) > ros::Duration(15.0)) {
00065       failed = true;
00066       done = true;
00067     }
00068     
00069     ROS_DEBUG_STREAM( "door open: " << open );
00070   }
00071   
00072   if(open) {
00073     CallGUI askToOpen("thank", CallGUI::DISPLAY,  "Thanks!");
00074     askToOpen.run();
00075     done = true;
00076   }
00077 
00078 }  
00079   
00080 actasp::Action* OpenDoor::cloneAndInit(const actasp::AspFluent& fluent) const {
00081   OpenDoor *newAction = new OpenDoor();
00082   newAction->door = fluent.getParameters().at(0);
00083   
00084   return newAction;
00085 }
00086 
00087 std::vector<std::string> OpenDoor::getParameters() const {
00088   vector<string> param;
00089   param.push_back(door);
00090   return param;
00091 }
00092 
00093 
00094 ActionFactory openDoorFactory(new OpenDoor());
00095   
00096 }


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