Go to the documentation of this file.00001 #include "bwi_kr_execution/ExecutePlanAction.h"
00002 #include <actionlib/client/simple_action_client.h>
00003 #include <ros/ros.h>
00004 #include "bwi_virtour/GoToLocation.h"
00005 #include "bwi_virtour/GoBesideLocation.h"
00006 #include "bwi_virtour/Authenticate.h"
00007 #include "bwi_virtour/Rotate.h"
00008
00009 typedef actionlib::SimpleActionClient<bwi_kr_execution::ExecutePlanAction> Client;
00010
00011 Client* client;
00012
00013 using namespace std;
00014
00015 bool goBesideLocation(bwi_virtour::GoBesideLocation::Request &req,
00016 bwi_virtour::GoBesideLocation::Response &res) {
00017 ROS_INFO("requesting goBesideLocation: %s", req.location.c_str());
00018
00019 ROS_INFO("authenticating user: %s", req.user.c_str());
00020 bwi_virtour::Authenticate::Request auth_req;
00021 auth_req.user = req.user;
00022 bwi_virtour::Authenticate::Response auth_res;
00023
00024 if (ros::service::call("/tour_manager/authenticate", auth_req, auth_res)) {
00025 if (auth_res.result < 0) {
00026 res.result = -5;
00027 ROS_INFO("Authentication failed!");
00028 return true;
00029 }
00030 }
00031
00032 ROS_INFO("waiting for server");
00033 client->waitForServer();
00034
00035 ROS_INFO("creating goal");
00036 bwi_kr_execution::ExecutePlanGoal goal;
00037
00038 bwi_kr_execution::AspRule rule;
00039 bwi_kr_execution::AspFluent fluent;
00040 fluent.name = "not beside";
00041
00042 fluent.variables.push_back(req.location);
00043
00044 rule.body.push_back(fluent);
00045 goal.aspGoal.push_back(rule);
00046
00047 ROS_INFO("sending goal");
00048 client->sendGoalAndWait(goal);
00049
00050 if (client->getState() == actionlib::SimpleClientGoalState::ABORTED) {
00051 ROS_INFO("Aborted");
00052 res.result = -3;
00053 } else if (client->getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
00054 ROS_INFO("Preempted");
00055 res.result = -2;
00056 } else if (client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00057 ROS_INFO("Succeeded!");
00058 res.result = 1;
00059 } else {
00060 ROS_INFO("Terminated");
00061 res.result = -1;
00062 }
00063
00064 ROS_INFO("sending back response: [%ld]", (long int)res.result);
00065 return true;
00066 }
00067
00068 bool goToLocation(bwi_virtour::GoToLocation::Request &req,
00069 bwi_virtour::GoToLocation::Response &res) {
00070 ROS_INFO("requesting goToLocation: %s", req.location.c_str());
00071
00072 ROS_INFO("authenticating user: %s", req.user.c_str());
00073 bwi_virtour::Authenticate::Request auth_req;
00074 auth_req.user = req.user;
00075 bwi_virtour::Authenticate::Response auth_res;
00076
00077 if (ros::service::call("/tour_manager/authenticate", auth_req, auth_res)) {
00078 if (auth_res.result < 0) {
00079 res.result = -5;
00080 ROS_INFO("Authentication failed!");
00081 return true;
00082 }
00083 }
00084
00085 ROS_INFO("waiting for server");
00086 client->waitForServer();
00087
00088 ROS_INFO("creating goal");
00089 bwi_kr_execution::ExecutePlanGoal goal;
00090
00091 bwi_kr_execution::AspRule rule;
00092 bwi_kr_execution::AspFluent fluent;
00093 fluent.name = "not at";
00094
00095 fluent.variables.push_back(req.location);
00096
00097 rule.body.push_back(fluent);
00098 goal.aspGoal.push_back(rule);
00099
00100 ROS_INFO("sending goal");
00101 client->sendGoalAndWait(goal);
00102
00103 if (client->getState() == actionlib::SimpleClientGoalState::ABORTED) {
00104 ROS_INFO("Aborted");
00105 res.result = -3;
00106 } else if (client->getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
00107 ROS_INFO("Preempted");
00108 res.result = -2;
00109 } else if (client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00110 ROS_INFO("Succeeded!");
00111 res.result = 1;
00112 } else {
00113 ROS_INFO("Terminated");
00114 res.result = -1;
00115 }
00116
00117 ROS_INFO("sending back response: [%ld]", (long int)res.result);
00118 return true;
00119 }
00120
00121
00122 int main(int argc, char**argv) {
00123 ros::init(argc, argv, "go_to_location_service_node");
00124 ros::NodeHandle n;
00125
00126 client = new Client("/action_executor/execute_plan", true);
00127
00128 ros::ServiceServer service = n.advertiseService("go_to_location", goToLocation);
00129 ROS_INFO("GoToLocation Service Started");
00130
00131 ros::ServiceServer service2 = n.advertiseService("go_beside_location", goBesideLocation);
00132 ROS_INFO("GoBesideLocation Service Started");
00133
00134 ros::spin();
00135 ROS_INFO("Done spinning");
00136 return 0;
00137 }