fixed_strategy.cpp
Go to the documentation of this file.
00001 #include "fixed_strategy.h"
00002 
00003 FixedStrategy::FixedStrategy(){
00004 
00005   //init class attributes if necessary
00006     this->execution_flag = false;
00007 
00008   //string for port names
00009 
00010   // [init publishers]
00011   this->focused_obj_label_publisher = this->node_handle_.advertise<std_msgs::Int32>("/planner/focused_obj_label", 5);
00012   
00013   // [init subscribers]
00014   
00015   // [init services]
00016   this->execution_flow_control_server = this->node_handle_.advertiseService("/planner/execution_flow_control", &FixedStrategy::execution_flow_controlCallback, this);
00017   
00018   // [init clients]
00019   obs_client = this->node_handle_.serviceClient<iri_wam_common_msgs::obs_request>("/planner/obs_client");
00020   wam_action_client = this->node_handle_.serviceClient<iri_wam_common_msgs::wamaction>("/planner/wam_action");
00021 
00022   // [init action servers]
00023   
00024   // [init action clients]
00025 }
00026 
00027 void FixedStrategy::mainLoop(void){
00028 
00029     if(execution_flag){
00030         ROS_INFO("Starting Loop");
00031         if(obs_client.call(obs_request_srv)) 
00032         { 
00033           ROS_INFO("Service result: %d objects at A, %d objects at B, total %d", obs_request_srv.response.num_objectsA,obs_request_srv.response.num_objectsB,obs_request_srv.response.num_objects); 
00034         } else { 
00035           ROS_ERROR("Failed to call service observation client"); 
00036           return;
00037         }
00038     
00039         //process observation
00040         if(obs_request_srv.response.num_objectsB > 1){
00041             //bring some back
00042             //unused on a fixed strategy
00043             this->ObjLabel_msg.data = 1; //1 = A, 2 = B
00044             focused_obj_label_publisher.publish(this->ObjLabel_msg);
00045             
00046             wam_actions_client_srv.request.action = CHANGEPILE;
00047             wam_actions_client_srv.request.zone = B;
00048             if(wam_action_client.call(wam_actions_client_srv)) { 
00049               ROS_INFO("Service result: %d", wam_actions_client_srv.response.success); 
00050             } else { 
00051               ROS_ERROR("Failed to call service wam actions service"); 
00052               return;
00053             }
00054         }else if(obs_request_srv.response.num_objectsB == 1){
00055             //hooray! take it out
00056             wam_actions_client_srv.request.action = MOVEAWAY;
00057             if(wam_action_client.call(wam_actions_client_srv)) { 
00058               ROS_INFO("Service result: %d", wam_actions_client_srv.response.success); 
00059             } else { 
00060               ROS_ERROR("Failed to call service wam actions service"); 
00061               return;
00062             }
00063         }else if(obs_request_srv.response.num_objectsB == 0){
00064             wam_actions_client_srv.request.action = CHANGEPILE;
00065             wam_actions_client_srv.request.zone = A;
00066             if(wam_action_client.call(wam_actions_client_srv)) { 
00067               ROS_INFO("Service result: %d", wam_actions_client_srv.response.success); 
00068             } else { 
00069               ROS_ERROR("Failed to call service wam actions service"); 
00070               return;
00071             }
00072         }
00073 
00074 
00075     }else{
00076         //sleep
00077         ROS_INFO("Awaiting trigger");
00078         sleep(5); 
00079     }
00080 
00081 }
00082 
00083 bool FixedStrategy::execution_flow_controlCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){
00084 
00085     //stop execution when called
00086     this->execution_flag = !this->execution_flag;
00087     //TODO free execution mutex
00088     return true;
00089 
00090 }
00091 
00092 /* main function */
00093 int main(int argc,char *argv[])
00094 {
00095     ros::init(argc, argv, "planner");
00096     FixedStrategy fixed_strategy;
00097     ros::Rate loop_rate(10); 
00098     while(ros::ok()){
00099       fixed_strategy.mainLoop();
00100       ros::spinOnce();
00101       loop_rate.sleep(); 
00102     }
00103 }


iri_dummy_pomdp
Author(s): pmonso
autogenerated on Fri Dec 6 2013 21:35:42