go_to_door.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, "go_to_door");
00014   ros::NodeHandle n;
00015   
00016   ros::NodeHandle privateNode("~");
00017   string door;
00018   privateNode.param<string>("door",door,"d3_414b1");
00019   
00020   ROS_INFO_STREAM("going to " << door);
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 facing";
00030   
00031   fluent.variables.push_back(door);
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 }


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