GoToStartPos.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <move_base_msgs/MoveBaseAction.h>
00003 #include <actionlib/client/simple_action_client.h>
00004 #include <tf/transform_broadcaster.h>
00005 #include <geometry_msgs/Twist.h>
00006 #include "patrolling_sim/GoToStartPosSrv.h" 
00007 
00008 
00009  typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
00010 
00011  using namespace std;
00012   
00013  ros::NodeHandle* n_ptr;
00014  int teamsize;
00015  double last_cmd_vel_time;
00016  
00017  
00018  void cmd_velCB(const geometry_msgs::Twist::ConstPtr& msg){
00019      //ROS_INFO("receiving cmd_vels");
00020      last_cmd_vel_time = ros::Time::now().toSec();
00021  }
00022  
00023 
00024  bool GotoStartPosSrvCallback(patrolling_sim::GoToStartPosSrv::Request& Req, patrolling_sim::GoToStartPosSrv::Response& Rep){
00025         
00026     if (Req.teamsize.data != teamsize) {
00027         ROS_INFO("Service was called with a different team size (%d) than expected (%d). Leaving.", Req.teamsize.data, teamsize);
00028         return false;
00029     }
00030     
00031     double starting_patrol_pos_x [teamsize];
00032     double starting_patrol_pos_y [teamsize];
00033    
00034     //list of doubles from the parameter server
00035     vector<double> initial_pos_list;
00036     n_ptr->getParam("initial_pos", initial_pos_list);
00037     
00038     unsigned i;
00039     int j=0;    //robot id
00040     
00041     for(i = 0; i < initial_pos_list.size(); i++) {        
00042         if ( i % 2 == 0 ){ //even: x
00043             starting_patrol_pos_x[j] = initial_pos_list[i];
00044             //ROS_INFO("starting_patrol_pos_x[%d] = %f", j, starting_patrol_pos_x[j]);
00045             
00046         }else{ //odd: y
00047             starting_patrol_pos_y[j] = initial_pos_list[i];
00048             //ROS_INFO("starting_patrol_pos_y[%d] = %f", j, starting_patrol_pos_y[j]);
00049             j++;
00050         }
00051     }
00052     
00053     
00054     //connect to move_base server and send robots one by one to starting positions...
00055     
00056     //array of pointers:
00057     MoveBaseClient *ac_ptr[teamsize];
00058     ros::Rate loop_rate(1); //1 sec
00059     
00060     for (j=teamsize-1; j>=0; j--){
00061         
00062         char move_string[20];
00063         sprintf(move_string,"robot_%d/move_base",j);
00064         //ROS_INFO("%s",move_string);
00065 
00066         
00067         MoveBaseClient ac(move_string, true);
00068         ac_ptr[j] = &ac;
00069         
00070         //wait for the action server to come up
00071         while(!ac.waitForServer(ros::Duration(5.0))){
00072             ROS_INFO("Waiting for the move_base action server to come up");
00073         } 
00074         ROS_INFO("Connected with move_base action server");     
00075         
00076         move_base_msgs::MoveBaseGoal goal;
00077         
00078         geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);  
00079 
00080         //we'll send a goal to the robot to move 1 meter forward
00081         goal.target_pose.header.frame_id = "/map";
00082         goal.target_pose.header.stamp = ros::Time::now();
00083         goal.target_pose.pose.position.x = starting_patrol_pos_x[j];
00084         goal.target_pose.pose.position.y = starting_patrol_pos_y[j];
00085         goal.target_pose.pose.orientation = angle_quat; //doesn't matter really.
00086 
00087         ROS_INFO("Sending goal");
00088         ac.sendGoal(goal);
00089         
00090         //wait a bit and send next goal to other robots.
00091         
00092         i=0;
00093         
00094         while( i<Req.sleep_between_goals.data ){
00095             i++;
00096             ros::spinOnce();    //trigger cmd_vel callbacks
00097             loop_rate.sleep(); 
00098         }
00099     }
00100     
00101     ROS_INFO("Let's make sure that all robots reach their goals...!");
00102 
00103     last_cmd_vel_time = ros::Time::now().toSec();   //safe initialization
00104     double current_time = ros::Time::now().toSec();
00105     
00106     while( current_time-last_cmd_vel_time < 5.0 ){ //check cmd_vels not received in the last 5 secs
00107         current_time = ros::Time::now().toSec();
00108         ros::spinOnce();
00109         loop_rate.sleep();
00110     }
00111     
00112     ROS_INFO("All robots successfully reached their starting positions");    
00113     return true;
00114  }
00115  
00116  
00117 int main(int argc, char **argv){
00118 
00119   ros::init(argc, argv, "GoToStartPos");
00120   ros::NodeHandle n;
00121   n_ptr = &n;
00122   
00123   ros::Duration(2.0).sleep();   //waiting for all navigation topics to show up.
00124    
00125   //get cmd_vel topics:
00126   ros::master::V_TopicInfo master_topics;
00127   ros::master::getTopics(master_topics);
00128   
00129   vector<string> cmd_vel_topic_array(32, ""); 
00130   teamsize=0;
00131 
00132   for (ros::master::V_TopicInfo::iterator it = master_topics.begin() ; it != master_topics.end(); it++) {
00133         const ros::master::TopicInfo& info = *it;
00134         if(info.datatype=="geometry_msgs/Twist"){   //cmd_vel topics
00135             cmd_vel_topic_array[teamsize] = info.name;
00136             teamsize++;
00137         }
00138   }
00139   
00140   //for(int o=0; o<teamsize; o++){ ROS_ERROR("%s",cmd_vel_topic_array[o].c_str()); }
00141   
00142   if (teamsize==0){
00143     ROS_ERROR("No navigation information retrieved. Is \"move_base\" running?");
00144     return -1;   
00145     
00146   }else{
00147       ROS_INFO("Detected %d robots.", teamsize);
00148   }
00149   
00150   vector<ros::Subscriber> cmd_vel_sub(teamsize);
00151   
00152   for(int o=0; o<teamsize; o++){ //create subscribers with a common callback:
00153       cmd_vel_sub[o] = n.subscribe(cmd_vel_topic_array[o], 1, cmd_velCB);
00154   }  
00155 
00156   ros::ServiceServer service = n.advertiseService("GotoStartPosSrv", GotoStartPosSrvCallback);
00157   ROS_WARN("Ready to send robots to starting position.");
00158   
00159   ros::spin();
00160 
00161   return 0;
00162 }


patrolling_sim
Author(s):
autogenerated on Mon Oct 2 2017 03:13:50