00001 #include "bwi_kr_execution/ExecutePlanAction.h" 00002 #include <actionlib/client/simple_action_client.h> 00003 #include <ros/ros.h> 00004 #include "bwi_services/GoToLocation.h" 00005 00006 typedef actionlib::SimpleActionClient<bwi_kr_execution::ExecutePlanAction> Client; 00007 00008 Client* client; 00009 00010 using namespace std; 00011 00012 bool go_to_location(bwi_services::GoToLocation::Request &req, 00013 bwi_services::GoToLocation::Response &res) { 00014 ROS_INFO("requesting goToLocation: %s", req.location.c_str()); 00015 00016 ROS_INFO("waiting for server"); 00017 client->waitForServer(); 00018 00019 ROS_INFO("creating goal"); 00020 bwi_kr_execution::ExecutePlanGoal goal; 00021 00022 bwi_kr_execution::AspRule rule; 00023 bwi_kr_execution::AspFluent fluent; 00024 fluent.name = "not at"; 00025 00026 fluent.variables.push_back(req.location); 00027 00028 rule.body.push_back(fluent); 00029 goal.aspGoal.push_back(rule); 00030 00031 ROS_INFO("sending goal"); 00032 client->sendGoalAndWait(goal); 00033 00034 if (client->getState() == actionlib::SimpleClientGoalState::ABORTED) { 00035 ROS_INFO("Aborted"); 00036 res.result = -3; 00037 } else if (client->getState() == actionlib::SimpleClientGoalState::PREEMPTED) { 00038 ROS_INFO("Preempted"); 00039 res.result = -2; 00040 } else if (client->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { 00041 ROS_INFO("Succeeded!"); 00042 res.result = 1; 00043 } else { 00044 ROS_INFO("Terminated"); 00045 res.result = -1; 00046 } 00047 00048 ROS_INFO("sending back response: [%ld]", (long int)res.result); 00049 return true; 00050 } 00051 00052 00053 int main(int argc, char**argv) { 00054 ros::init(argc, argv, "go_to_location_service_node"); 00055 ros::NodeHandle n; 00056 00057 client = new Client("/action_executor/execute_plan", true); 00058 00059 ros::ServiceServer service = n.advertiseService("/bwi_services/go_to_location", go_to_location); 00060 ROS_INFO("GoToLocation Service Started"); 00061 00062 ros::spin(); 00063 ROS_INFO("Done spinning"); 00064 return 0; 00065 }