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
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
00035 vector<double> initial_pos_list;
00036 n_ptr->getParam("initial_pos", initial_pos_list);
00037
00038 unsigned i;
00039 int j=0;
00040
00041 for(i = 0; i < initial_pos_list.size(); i++) {
00042 if ( i % 2 == 0 ){
00043 starting_patrol_pos_x[j] = initial_pos_list[i];
00044
00045
00046 }else{
00047 starting_patrol_pos_y[j] = initial_pos_list[i];
00048
00049 j++;
00050 }
00051 }
00052
00053
00054
00055
00056
00057 MoveBaseClient *ac_ptr[teamsize];
00058 ros::Rate loop_rate(1);
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
00065
00066
00067 MoveBaseClient ac(move_string, true);
00068 ac_ptr[j] = ∾
00069
00070
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
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;
00086
00087 ROS_INFO("Sending goal");
00088 ac.sendGoal(goal);
00089
00090
00091
00092 i=0;
00093
00094 while( i<Req.sleep_between_goals.data ){
00095 i++;
00096 ros::spinOnce();
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();
00104 double current_time = ros::Time::now().toSec();
00105
00106 while( current_time-last_cmd_vel_time < 5.0 ){
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();
00124
00125
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"){
00135 cmd_vel_topic_array[teamsize] = info.name;
00136 teamsize++;
00137 }
00138 }
00139
00140
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++){
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 }