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 }