between_doors.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   string locationA;
00018   privateNode.param<string>("a",locationA,"d3_414b1");
00019 
00020   string locationB;
00021   privateNode.param<string>("b",locationB,"d3_414b2");
00022 
00023 
00024 
00025   Client client("/action_executor/execute_plan", true);
00026   client.waitForServer();
00027 
00028   bool fromAtoB = true;
00029 
00030   while (ros::ok()) {
00031 
00032     string location = (fromAtoB)? locationB : locationA;
00033 
00034     fromAtoB = !fromAtoB;
00035 
00036     ROS_INFO_STREAM("going to " << location);
00037 
00038     bwi_kr_execution::ExecutePlanGoal goal;
00039 
00040     bwi_kr_execution::AspRule rule;
00041     bwi_kr_execution::AspFluent fluent;
00042     fluent.name = "not facing";
00043 
00044     fluent.variables.push_back(location);
00045 
00046     rule.body.push_back(fluent);
00047     goal.aspGoal.push_back(rule);
00048 
00049     ROS_INFO("sending goal");
00050     client.sendGoalAndWait(goal);
00051 
00052     if (client.getState() == actionlib::SimpleClientGoalState::ABORTED) {
00053       ROS_INFO("Aborted");
00054     } else if (client.getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
00055       ROS_INFO("Preempted");
00056     }
00057 
00058     else if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00059       ROS_INFO("Succeeded!");
00060     } else
00061       ROS_INFO("Terminated");
00062 
00063   }
00064 
00065   return 0;
00066 }


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