Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include "bwi_kr_execution/ExecutePlanAction.h"
00009
00010 #include <actionlib/client/simple_action_client.h>
00011
00012 #include <ros/ros.h>
00013
00014 #include <bwi_msgs/QuestionDialog.h>
00015
00016 typedef actionlib::SimpleActionClient<bwi_kr_execution::ExecutePlanAction> Client;
00017
00018 using namespace std;
00019
00020 int main(int argc, char**argv) {
00021 ros::init(argc, argv, "visit_door_list_gui");
00022 ros::NodeHandle n;
00023
00024 ros::NodeHandle privateNode("~");
00025
00026 std::vector<string> doors;
00027
00028 doors.push_back("d3_414b1");
00029 doors.push_back("d3_414b2");
00030 doors.push_back("d3_414a1");
00031 doors.push_back("d3_414a2");
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 ros::ServiceClient client_gui = n.serviceClient<bwi_msgs::QuestionDialog>("/question_dialog");
00039
00040 while (ros::ok()) {
00041
00042
00043 bwi_msgs::QuestionDialog question;
00044 question.request.type = question.request.CHOICE_QUESTION;
00045
00046 question.request.message = "Please select the next door:";
00047 for (unsigned int k = 0; k < doors.size(); k++){
00048 question.request.options.push_back(doors.at(k));
00049 }
00050 question.request.timeout = 30.0;
00051
00052 if (client_gui.call(question))
00053 {
00054 if (question.response.index >= 0){
00055 current_door = question.response.index;
00056 }
00057 else {
00058 ROS_INFO("No response detected, defaulting to 0");
00059 current_door = 0;
00060 }
00061 }
00062 else
00063 {
00064 ROS_ERROR("Failed to call service /question_dialog");
00065 return 1;
00066 }
00067
00068 string location = doors.at(current_door);
00069
00070 ROS_INFO_STREAM("going to " << location);
00071
00072 bwi_kr_execution::ExecutePlanGoal goal;
00073
00074 bwi_kr_execution::AspRule rule;
00075 bwi_kr_execution::AspFluent fluent;
00076 fluent.name = "not facing";
00077
00078 fluent.variables.push_back(location);
00079
00080 rule.body.push_back(fluent);
00081 goal.aspGoal.push_back(rule);
00082
00083 ROS_INFO("sending goal");
00084 client.sendGoalAndWait(goal);
00085
00086 if (client.getState() == actionlib::SimpleClientGoalState::ABORTED) {
00087 ROS_INFO("Aborted");
00088 } else if (client.getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
00089 ROS_INFO("Preempted");
00090 }
00091
00092 else if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00093 ROS_INFO("Succeeded!");
00094 } else
00095 ROS_INFO("Terminated");
00096
00097 }
00098
00099 return 0;
00100 }