ChangeFloor.cpp
Go to the documentation of this file.
00001 #include "ChangeFloor.h"
00002 
00003 #include <boost/foreach.hpp>
00004 
00005 #include "ActionFactory.h"
00006 #include "../StaticFacts.h"
00007 
00008 #include "bwi_kr_execution/UpdateFluents.h"
00009 #include "ros/console.h"
00010 #include "ros/ros.h"
00011 
00012 #include <actionlib/client/simple_action_client.h>
00013 #include <bwi_msgs/LogicalNavigationAction.h>
00014 
00015 namespace bwi_krexec {
00016 
00017 ChangeFloor::ChangeFloor() :
00018              done(false),
00019              asked(false),
00020              failed(false) {}
00021 
00022 void ChangeFloor::run() {
00023   
00024   if(!asked) {
00025 
00026     // Get the doors for this elevator.
00027     std::string dest_floor;
00028     std::list<actasp::AspAtom> static_facts = StaticFacts::staticFacts(); 
00029     BOOST_FOREACH(const actasp::AspAtom fact, static_facts) {
00030       if (fact.getName() == "floor") {
00031         std::vector<std::string> params = fact.getParameters();
00032         if (params[0] == dest_room) {
00033           dest_floor = params[1];
00034           break;
00035         }
00036       }
00037     }
00038 
00039     if (dest_floor.empty()) {
00040       ROS_ERROR_STREAM("Unable to retrieve floor for destination " << dest_room << ". Cannot complete action!");
00041       done = true;
00042       failed = true;
00043     } else {
00044       std::vector<std::string> options;
00045       options.push_back("Reached!");
00046       askToChangeFloor.reset(new CallGUI("askToChangeFloor", 
00047                                          CallGUI::CHOICE_QUESTION,  
00048                                          "Could you press the button for floor " + dest_floor + 
00049                                            ", and then let me know when the elevator arrives there?", 
00050                                          120.0f, 
00051                                          options));
00052       askToChangeFloor->run();
00053     }
00054     asked = true;
00055   } else if(!done) {
00056     if (askToChangeFloor->hasFinished()) {
00057       // Check response to see it's positive.
00058       int response_idx = askToChangeFloor->getResponseIndex();
00059       if (response_idx == 0) {
00060 
00061         // Get the doors for this elevator.
00062         std::string facing_door;
00063         std::list<actasp::AspAtom> static_facts = StaticFacts::staticFacts(); 
00064         BOOST_FOREACH(const actasp::AspAtom fact, static_facts) {
00065           if (fact.getName() == "hasdoor") {
00066             std::vector<std::string> params = fact.getParameters();
00067             if (params[0] == dest_room) {
00068               // NOTE: This makes the assumption that an elevator room only has a single door, which is true for GDC.
00069               facing_door = params[1];
00070               break;
00071             }
00072           }
00073         }
00074 
00075         if (facing_door.empty()) {
00076           ROS_ERROR_STREAM("Unable to retrieve door we're facing for destination " << dest_room << ". Cannot complete action!");
00077           failed = true;
00078         } else {
00079 
00080           // Attempt to change the robot's location to this floor and location.
00081           boost::shared_ptr<actionlib::SimpleActionClient<bwi_msgs::LogicalNavigationAction> > lnac;
00082           lnac.reset(new actionlib::SimpleActionClient<bwi_msgs::LogicalNavigationAction>("execute_logical_goal",
00083                                                                                                            true));
00084           lnac->waitForServer();
00085           bwi_msgs::LogicalNavigationGoal goal;
00086           goal.command.name = "changefloor";
00087           goal.command.value.push_back(dest_room);
00088           goal.command.value.push_back(facing_door);
00089           lnac->sendGoal(goal);
00090           lnac->waitForResult();
00091 
00092           // TODO incorporate the return of the changefloor command as well.
00093           ros::NodeHandle n;
00094           ros::ServiceClient krClient = n.serviceClient<bwi_kr_execution::UpdateFluents> ( "update_fluents" );
00095           krClient.waitForExistence();
00096           bwi_kr_execution::UpdateFluents uf;
00097 
00098           bwi_kr_execution::AspFluent open_door;
00099           open_door.name = "open";
00100           open_door.variables.push_back(facing_door);
00101 
00102           bwi_kr_execution::AspFluent face_door;
00103           face_door.name = "facing";
00104           face_door.variables.push_back(facing_door);
00105 
00106           bwi_kr_execution::AspFluent beside_door;
00107           beside_door.name = "beside";
00108           beside_door.variables.push_back(facing_door);
00109 
00110           bwi_kr_execution::AspFluent at_loc;
00111           at_loc.name = "at";
00112           at_loc.variables.push_back(dest_room);
00113 
00114           uf.request.fluents.push_back(open_door);
00115           uf.request.fluents.push_back(face_door);
00116           uf.request.fluents.push_back(beside_door);
00117           uf.request.fluents.push_back(at_loc);
00118           krClient.call(uf);
00119 
00120           CallGUI thanks("thanks", CallGUI::DISPLAY,  "Thanks! Could you keep the door open while I exit the elevator?");
00121           thanks.run();
00122         }
00123       } else {
00124         // A door didn't open in the timeout specified.
00125         failed = true;
00126       }
00127       done = true;
00128     }
00129   }
00130  
00131 }
00132 
00133 bool ChangeFloor::hasFinished() const {
00134   return done;
00135 }
00136 
00137 bool ChangeFloor::hasFailed() const {
00138   return failed;
00139 }
00140 
00141 actasp::Action *ChangeFloor::cloneAndInit(const actasp::AspFluent & fluent) const {
00142   ChangeFloor *other = new ChangeFloor();
00143   other->dest_room = fluent.getParameters().at(0);
00144   return other;
00145 }
00146 
00147 std::vector<std::string> ChangeFloor::getParameters() const {
00148   return std::vector<std::string>(1,dest_room);
00149 }
00150 
00151 //if you want the action to be available only in simulation, or only
00152 //on the robot, use the constructor that also takes a boolean.
00153 ActionFactory changeFloor(new ChangeFloor(), false);
00154 
00155 }


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