OpenSimulatedDoor.cpp
Go to the documentation of this file.
00001 #include "OpenSimulatedDoor.h"
00002 
00003 #include "bwi_kr_execution/CurrentStateQuery.h"
00004 #include "bwi_msgs/DoorHandlerInterface.h"
00005 
00006 #include "ActionFactory.h"
00007 #include "LogicalNavigation.h"
00008 
00009 #include "actasp/AspFluent.h"
00010 
00011 #include <ros/ros.h>
00012 
00013 using namespace std;
00014 using namespace ros;
00015 
00016 namespace bwi_krexec {
00017 
00018 
00019 OpenSimulatedDoor::OpenSimulatedDoor() : door(), done(false),requestSent(false) {}
00020 
00021 void OpenSimulatedDoor::run() {
00022   NodeHandle n;
00023 
00024   if (!requestSent) {
00025     ServiceClient doorClient = n.serviceClient<bwi_msgs::DoorHandlerInterface> ("/update_doors");
00026     doorClient.waitForExistence();
00027 
00028     bwi_msgs::DoorHandlerInterface dhi;
00029 
00030     dhi.request.all_doors = false;
00031     dhi.request.door = door;
00032     dhi.request.open = true;
00033 
00034     doorClient.call(dhi);
00035 
00036     requestSent = true;
00037   }
00038 
00039   vector<string> params;
00040   params.push_back(door);
00041   LogicalNavigation senseDoor("sensedoor",params);
00042 
00043   senseDoor.run();
00044 
00045   ros::ServiceClient currentClient = n.serviceClient<bwi_kr_execution::CurrentStateQuery> ("current_state_query");
00046   bwi_kr_execution::AspFluent openFluent;
00047   openFluent.name = "open";
00048   openFluent.timeStep = 0;
00049   openFluent.variables.push_back(door);
00050 
00051   bwi_kr_execution::AspRule rule;
00052   rule.head.push_back(openFluent);
00053 
00054   bwi_kr_execution::CurrentStateQuery csq;
00055   csq.request.query.push_back(rule);
00056 
00057   currentClient.call(csq);
00058 
00059   done = csq.response.answer.satisfied;
00060 
00061 }
00062 
00063 
00064 actasp::Action* OpenSimulatedDoor::cloneAndInit(const actasp::AspFluent& fluent) const {
00065   OpenSimulatedDoor *newAction = new OpenSimulatedDoor();
00066   newAction->door = fluent.getParameters().at(0);
00067 
00068   return newAction;
00069 }
00070 
00071 std::vector<std::string> OpenSimulatedDoor::getParameters() const {
00072   vector<string> param;
00073   param.push_back(door);
00074   return param;
00075 }
00076 
00077 bwi_krexec::ActionFactory simulatedOpenDoor(new OpenSimulatedDoor(), true);
00078 
00079 }


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