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/Authenticate.h"
00006 #include "geometry_msgs/Twist.h"
00007 #include "bwi_virtour/Rotate.h"
00008
00009 typedef actionlib::SimpleActionClient<bwi_kr_execution::ExecutePlanAction> Client;
00010
00011 Client* client;
00012 ros::Publisher cmd_vel_pub;
00013
00014 using namespace std;
00015
00016 bool rotateRequest(bwi_virtour::Rotate::Request &req,
00017 bwi_virtour::Rotate::Response &res) {
00018
00019 ROS_INFO("Received rotate request");
00020 float rotateDelta = req.rotateDelta;
00021 if (rotateDelta > 2) {
00022 rotateDelta = 2;
00023 } else if (rotateDelta < -2) {
00024 rotateDelta = -2;
00025 }
00026
00027 ROS_INFO("authenticating user: %s", req.user.c_str());
00028 bwi_virtour::Authenticate::Request auth_req;
00029 auth_req.user = req.user;
00030 bwi_virtour::Authenticate::Response auth_res;
00031
00032 if (ros::service::call("/tour_manager/authenticate", auth_req, auth_res)) {
00033 if (auth_res.result < 0) {
00034 res.result = -5;
00035 ROS_INFO("Authentication failed!");
00036 return true;
00037 }
00038 }
00039
00040 geometry_msgs::Twist msg;
00041 msg.angular.z = rotateDelta;
00042 cmd_vel_pub.publish(msg);
00043
00044 res.result = 1;
00045 return true;
00046 }
00047
00048 int main(int argc, char**argv) {
00049 ros::init(argc, argv, "rotate_service_node");
00050 ros::NodeHandle n;
00051
00052 cmd_vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);
00053 ros::ServiceServer service = n.advertiseService("rotate", rotateRequest);
00054 ROS_INFO("Rotate Service Started");
00055
00056 ros::spin();
00057 ROS_INFO("Done spinning");
00058 return 0;
00059 }