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, "go_to_location"); 00014 ros::NodeHandle n; 00015 00016 ros::NodeHandle privateNode("~"); 00017 string location; 00018 privateNode.param<string>("location",location,"l3_414b"); 00019 00020 ROS_INFO_STREAM("going to " << location); 00021 00022 Client client("action_executor/execute_plan", true); 00023 client.waitForServer(); 00024 00025 bwi_kr_execution::ExecutePlanGoal goal; 00026 00027 bwi_kr_execution::AspRule rule; 00028 bwi_kr_execution::AspFluent fluent; 00029 fluent.name = "not at"; 00030 00031 fluent.variables.push_back(location); 00032 00033 rule.body.push_back(fluent); 00034 goal.aspGoal.push_back(rule); 00035 00036 ROS_INFO("sending goal"); 00037 client.sendGoal(goal); 00038 00039 ros::Rate wait_rate(10); 00040 while(ros::ok() && !client.getState().isDone()) 00041 wait_rate.sleep(); 00042 00043 if (!client.getState().isDone()) { 00044 ROS_INFO("Canceling goal"); 00045 client.cancelGoal(); 00046 //and wait for canceling confirmation 00047 for(int i = 0; !client.getState().isDone() && i<50; ++i) 00048 wait_rate.sleep(); 00049 } 00050 if (client.getState() == actionlib::SimpleClientGoalState::ABORTED) { 00051 ROS_INFO("Aborted"); 00052 } 00053 else if (client.getState() == actionlib::SimpleClientGoalState::PREEMPTED) { 00054 ROS_INFO("Preempted"); 00055 } 00056 00057 else if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { 00058 ROS_INFO("Succeeded!"); 00059 } 00060 else 00061 ROS_INFO("Terminated"); 00062 00063 return 0; 00064 }