1 #include "ros/ros.h"
   2 #include "geometry_msgs/Pose.h"
   3 #include "tf/transform_listener.h"
   4 #include "tf/transform_broadcaster.h"
   5 #include "biped_robin_msgs/PickupObjectService.h"
   6 #include "biped_robin_msgs/PlaceObjectService.h"
   7 #include "std_srvs/Empty.h"
   8 #include "std_msgs/String.h"
   9 #include <string>
  10 
  11 ros::ServiceClient pickup_client;
  12 ros::ServiceClient place_client;
  13 ros::ServiceClient start_detecting_client;
  14 ros::ServiceClient stop_detecting_client;
  15 ros::Publisher chatter_pub;
  16 static tf::TransformListener* pListener = NULL;
  17 static float PI = 3.14159265359;
  18 
  19 //Becher aufnehmen
  20 bool pickupCylinder(){
  21 	biped_robin_msgs::PickupObjectService srv;
  22 	srv.request.object.reference_frame_id = "cylinder_center";	
  23 	pickup_client.call(srv);
  24 	return true;
  25 };
  26 
  27 //Becher entsorgen
  28 bool placeCylinder(){
  29 	biped_robin_msgs::PickupObjectService srv;
  30 	srv.request.object.reference_frame_id = "bin_frame";
  31 	place_client.call(srv);
  32 	return true;	
  33 };
  34 
  35 
  36 //Calbacks
  37 bool pickupCylinderCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
  38 	ROS_INFO("pickup_cylinder_srv called");	
  39 	return pickupCylinder();
  40 }
  41 
  42 bool placeCylinderCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
  43 	ROS_INFO("place_cylinder_srv called");		
  44 	return placeCylinder();
  45 }
  46 
  47 
  48 
  49 int main(int argc, char **argv)
  50 {
  51   /* init ROS */    
  52   ros::init(argc, argv, "biped_localizer");
  53 	pListener = new(tf::TransformListener);
  54 	ros::NodeHandle n;
  55 
  56 	//service servers	
  57 	ros::ServiceServer pickup_cylinder_srv = n.advertiseService("pickup_cylinder_srv" , pickupCylinderCallback);
  58 	ros::ServiceServer place_cylinder_srv = n.advertiseService("place_cylinder_srv" , placeCylinderCallback);
  59 
  60 	//service clients
  61 	pickup_client = n.serviceClient<biped_robin_msgs::PickupObjectService>("pickup_srv");
  62 	place_client = n.serviceClient<biped_robin_msgs::PickupObjectService>("place_srv");
  63 	start_detecting_client = n.serviceClient<std_srvs::Empty>("start_detecting_srv");
  64 	stop_detecting_client = n.serviceClient<std_srvs::Empty>("stop_detecting_srv");
  65 
  66 	//publisher	
  67 	chatter_pub = n.advertise<std_msgs::String>("/speech", 1000);
  68 
  69 	tf::StampedTransform odom_cylinder_transform;
  70 	std_srvs::Empty srv;
  71 	ros::Rate loop_rate(1);	
  72 	while(ros::ok){	
  73 		
  74 		bool move = false;
  75 		start_detecting_client.call(srv);
  76 		ros::Time now = ros::Time::now();
  77 		try {
  78 			//auf erkennung des bechers warten
  79 			pListener->waitForTransform("/odom", "/cylinder_center", now, ros::Duration(2.0));
  80     	pListener->lookupTransform("/odom", "/cylinder_center", now, odom_cylinder_transform);				
  81 			ROS_INFO("CUP FOUND");
  82 			stop_detecting_client.call(srv);
  83 			std_msgs::String msg;
  84 			msg.data = "I found a cup";
  85 			chatter_pub.publish(msg);
  86 			pickupCylinder();
  87 			msg.data = "I will throw away the cup for you";
  88 			chatter_pub.publish(msg);
  89 			placeCylinder();	
  90 			msg.data = "Feed me with another cup";
  91 			chatter_pub.publish(msg);	
  92 		} catch (tf::TransformException ex) {
  93 			ROS_INFO("NO CUP FOUND");
  94 		}	
  95 		loop_rate.sleep();		
  96 		ros::spinOnce();
  97 	}
  98   return 0;
  99 }