visit_door_list.cpp
Go to the documentation of this file.
00001 
00002 #include "bwi_kr_execution/ExecutePlanAction.h"
00003 
00004 #include <actionlib/client/simple_action_client.h>
00005 
00006 #include <ros/ros.h>
00007 
00008 typedef actionlib::SimpleActionClient<bwi_kr_execution::ExecutePlanAction> Client;
00009 
00010 using namespace std;
00011 
00012 int main(int argc, char**argv) {
00013   ros::init(argc, argv, "between_doors");
00014   ros::NodeHandle n;
00015 
00016   ros::NodeHandle privateNode("~");
00017   
00018   
00019   /*string locationA;
00020   privateNode.param<string>("a",locationA,"d3_414b1");
00021 
00022   string locationB
00023   privateNode.param<string>("b",locationB,"d3_414b2");*/
00024   
00025   std::vector<string> doors;
00026   
00027   doors.push_back("d3_414b1");
00028   doors.push_back("d3_414b2");
00029   doors.push_back("d3_414a1");
00030   doors.push_back("d3_414a2");
00031 
00032   doors.push_back("d3_418");
00033   int current_door = 0;
00034 
00035   Client client("/action_executor/execute_plan", true);
00036   client.waitForServer();
00037 
00038   bool fromAtoB = true;
00039 
00040   while (ros::ok()) {
00041 
00042     string location = doors.at(current_door);
00043     current_door++;
00044     if (current_door >= (int)doors.size())
00045         current_door = 0;
00046 
00047    
00048 
00049     ROS_INFO_STREAM("going to " << location);
00050 
00051     bwi_kr_execution::ExecutePlanGoal goal;
00052 
00053     bwi_kr_execution::AspRule rule;
00054     bwi_kr_execution::AspFluent fluent;
00055     fluent.name = "not facing";
00056 
00057     fluent.variables.push_back(location);
00058 
00059     rule.body.push_back(fluent);
00060     goal.aspGoal.push_back(rule);
00061 
00062     ROS_INFO("sending goal");
00063     client.sendGoalAndWait(goal);
00064 
00065     if (client.getState() == actionlib::SimpleClientGoalState::ABORTED) {
00066       ROS_INFO("Aborted");
00067     } else if (client.getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
00068       ROS_INFO("Preempted");
00069     }
00070 
00071     else if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00072       ROS_INFO("Succeeded!");
00073     } else
00074       ROS_INFO("Terminated");
00075 
00076   }
00077 
00078   return 0;
00079 }


bwi_tasks
Author(s): Matteo Leonetti, Shiqi Zhang
autogenerated on Thu Jun 6 2019 17:58:00