00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <stdlib.h>
00042 #include <stdio.h>
00043 #include <math.h>
00044 #include <string.h>
00045 #include <unistd.h>
00046
00047
00048
00049
00050
00051 #include <ros/ros.h>
00052 #include <ros/time.h>
00053 #include <std_msgs/String.h>
00054 #include <std_msgs/Float32.h>
00055 #include <std_msgs/Int8.h>
00056 #include <std_msgs/Int16.h>
00057 #include <std_msgs/Bool.h>
00058 #include <std_msgs/Empty.h>
00059 #include <geometry_msgs/Point32.h>
00060 #include <geometry_msgs/Twist.h>
00061 #include <nav_msgs/Odometry.h>
00062 #include <sensor_msgs/LaserScan.h>
00063
00064 #define PI 3.14159265
00065
00066 #define sonarSamples 1
00067 #define maxRange 1.5
00068 #define rotateSpeed 0.4
00069 #define moveSpeed 0.3
00070 #define reactiveRange 0.5
00071
00072 #define navigation REACTIVE // COGNITIVE
00073
00074 ros::Publisher cmd_velocidade;
00075
00076 ros::Subscriber odm_stage;
00077 ros::Subscriber sonarsRange_stage;
00078
00079
00080 sensor_msgs::LaserScan sonarsRange;
00081
00082
00083
00084 float leftSonar, rightSonar, centerSonar;
00085 float x=0, y=0;
00086 double theta=0;
00087 double thetaPol=0;
00088
00089
00090 float x_goTo=0, y_goTo=0, theta_goTo=0;
00091
00092
00093
00094 void odom_readCB(const nav_msgs::Odometry::ConstPtr& odom) {
00095
00096 x = odom->pose.pose.position.x;
00097 y = odom->pose.pose.position.y;
00098 theta = odom->pose.pose.orientation.w;
00099 thetaPol = odom->pose.pose.orientation.z;
00100
00101
00102
00103
00104 }
00105
00106
00107 float minValue(int ini_idx, int final_idx, float tab[], float maxValue ) {
00108
00109 int idx;
00110 float min= tab[ini_idx];
00111
00112 for (idx=ini_idx; idx < final_idx; idx++) {
00113
00114 if (tab[idx] < maxValue && tab[idx] < min) {
00115
00116 min = tab[idx];
00117 }
00118 }
00119 return min;
00120 }
00121
00122
00123
00124 void sonars_readCB(const sensor_msgs::LaserScan::ConstPtr& range) {
00125
00126 float tab[3*sonarSamples];
00127
00128 for (int i=0; i<3*sonarSamples; i++){
00129 tab[i]=range->ranges[i];
00130 }
00131
00132 rightSonar = minValue(0, sonarSamples, tab, maxRange);
00133 centerSonar = minValue(sonarSamples, 2*sonarSamples, tab, maxRange);
00134 leftSonar = minValue(2*sonarSamples, 3*sonarSamples, tab, maxRange);
00135
00136
00137
00138 }
00139
00140
00141 double quaternion2degrees(double angle, double polarization) {
00142
00143
00144
00145 double angle_degrees;
00146 int fact=0;
00147
00148 angle_degrees=2*( (acos (angle) * 180) / PI );
00149
00150 if (polarization < 0) {
00151 fact=-1;
00152 } else {
00153 fact=1;
00154 }
00155
00156 return angle_degrees=fact*angle_degrees;
00157 }
00158
00159
00160
00161 void move(float distance, float speed){
00162
00163
00164
00165
00166 float calDist, x0 , y0;
00167
00168 x0= x;
00169 y0= y;
00170 calDist= 0;
00171
00172 geometry_msgs::Twist speedM;
00173
00174 speedM.linear.x= speed;
00175
00176 distance=distance-(moveSpeed/10);
00177
00178 while( calDist < distance ){
00179
00180 calDist=sqrt( pow(x-x0,2) + pow(y-y0,2) );
00181
00182 cmd_velocidade.publish(speedM);
00183 ros::spinOnce();
00184
00185
00186 }
00187
00188
00189 speedM.linear.x= 0;
00190 cmd_velocidade.publish(speedM);
00191 ros::spinOnce();
00192
00193
00194 }
00195
00196
00197
00198 void rotate(float angle_deg, float speed){
00199
00200
00201
00202 double ang=0;
00203 float finalAng=0;
00204 int direction=0;
00205
00206
00207
00208
00209 geometry_msgs::Twist speedR;
00210
00211 if (angle_deg < 0 ){
00212 angle_deg=angle_deg+(rotateSpeed*10);
00213 speedR.angular.z= -speed;
00214 direction = -1;
00215
00216 } else {
00217 angle_deg=angle_deg-(rotateSpeed*10);
00218 speedR.angular.z= speed;
00219 direction = 1;
00220
00221 }
00222
00223
00224 finalAng = quaternion2degrees( theta, thetaPol ) + angle_deg;
00225
00226 if ( finalAng > 180 ) {
00227
00228 finalAng = finalAng - 360;
00229
00230 } else if ( finalAng < (-180) ) {
00231
00232 finalAng = finalAng + 360;
00233 }
00234
00235
00236 ang = quaternion2degrees( theta , thetaPol);
00237
00238
00239
00241 if( direction > 0 ) {
00242
00243 if ( ang > 0 && finalAng < 0 ) {
00244
00245 while (ang > finalAng){
00246 cmd_velocidade.publish(speedR);
00247 ros::spinOnce();
00248 ang= quaternion2degrees( theta , thetaPol);
00249
00250
00251 if (ang < 0 ) {
00252
00253 while (ang < finalAng){
00254 cmd_velocidade.publish(speedR);
00255 ros::spinOnce();
00256 ang= quaternion2degrees( theta , thetaPol);
00257 }
00258 break;
00259 }
00260 }
00261
00262 } else {
00263
00264 while( ang < finalAng ){
00265 cmd_velocidade.publish(speedR);
00266 ros::spinOnce();
00267 ang= quaternion2degrees( theta , thetaPol);
00268
00269 }
00270 }
00271
00272 } else {
00273
00275
00276 if ( ang < 0 && finalAng > 0 ) {
00277
00278 while (ang < finalAng){
00279 cmd_velocidade.publish(speedR);
00280 ros::spinOnce();
00281 ang= quaternion2degrees( theta , thetaPol);
00282
00283 }
00284
00285 } else {
00286
00287 while( ang > finalAng ){
00288 cmd_velocidade.publish(speedR);
00289 ros::spinOnce();
00290 ang= quaternion2degrees( theta , thetaPol);
00291
00292 }
00293 }
00294
00295 }
00296
00297
00298 speedR.angular.z= 0;
00299 cmd_velocidade.publish(speedR);
00300 ros::spinOnce();
00301
00302
00303 }
00304
00305
00306 void reactiveNavigation() {
00307
00308 int cnt=0;
00309
00310 geometry_msgs::Twist speedM;
00311
00312 speedM.linear.x= moveSpeed ;
00313
00314 while (cnt < 1000) {
00315
00316 usleep(100000);
00317
00318
00319 cmd_velocidade.publish(speedM);
00320 ros::spinOnce();
00321
00322 while (rightSonar <= reactiveRange || leftSonar<=reactiveRange || centerSonar<=reactiveRange) {
00323
00324 usleep(100000);
00325
00326
00327
00328 if (rightSonar <= leftSonar) {
00329
00330 while (centerSonar<=reactiveRange || rightSonar<=reactiveRange) {
00331 usleep(100000);
00332 rotate(10,rotateSpeed);
00333
00334 }
00335
00336
00337 } else {
00338
00339 while (centerSonar<=reactiveRange || leftSonar<=reactiveRange) {
00340 usleep(100000);
00341 rotate(-10,rotateSpeed);
00342 }
00343 }
00344
00345 }
00346
00347
00348
00349 ++cnt;
00350
00351 }
00352
00353
00354 }
00355
00356
00357
00358
00359 void goTo(float xIni, float yIni, float teta, float xFinal, float yFinal, float tetaFinal){
00360
00361
00362 float dist=0, alfa=0, phi=0;
00363
00364
00365
00366
00367 if ( xIni==xFinal && yIni==yFinal) {
00368
00369 phi = tetaFinal - teta;
00370 rotate(phi,rotateSpeed);
00371
00372
00373 } else {
00374
00375 dist=sqrt(pow(xFinal-xIni,2)+pow(yFinal-yIni,2));
00376 alfa=atan2((yFinal-yIni),(xFinal-xIni))*180.0/PI;
00377 phi=alfa-teta;
00378
00379
00380
00381
00382 if (abs(phi)>180)
00383 {
00384 phi=phi-(abs(phi)/phi)*360.0;
00385 }
00386
00387
00388 rotate(phi,rotateSpeed);
00389
00390
00391
00392 move(dist,moveSpeed);
00393
00394 }
00395
00396
00397
00398 x_goTo = xFinal;
00399 y_goTo = yFinal;
00400 theta_goTo = theta_goTo + phi;
00401
00402
00403
00404 }
00405
00406
00407 int main(int argc, char** argv)
00408 {
00409
00410
00411 int ID_ROBOT=0;
00412 ID_ROBOT = atoi(argv[1]);
00413 char path1[]="/robot_";
00414 char path2[]="/cmd_vel";
00415 char path3[]="/base_scan";
00416 char path4[]="/odom";
00417 char name[]="TraxBot_";
00418
00419 char path_cmdVel[30];
00420 char path_baseScan[30];
00421 char path_odm[30];
00422 char node_name[30];
00423
00424
00425
00426 sprintf(path_cmdVel,"%s%d%s",path1,ID_ROBOT,path2);
00427 sprintf(path_baseScan,"%s%d%s",path1,ID_ROBOT,path3);
00428 sprintf(path_odm,"%s%d%s",path1,ID_ROBOT,path4);
00429 sprintf(node_name,"%s%d",name,ID_ROBOT);
00430
00431
00432 ros::init(argc, argv, node_name);
00433 ros::NodeHandle nh;
00434
00435 ROS_INFO("ROS in TraxBot: Start ROS move in stage...");
00436
00437
00438
00439 sonarsRange_stage = nh.subscribe(path_baseScan, 1, sonars_readCB);
00440 odm_stage = nh.subscribe(path_odm, 1, odom_readCB);
00441
00442
00443 cmd_velocidade = nh.advertise<geometry_msgs::Twist>(path_cmdVel, 1);
00444
00445
00446 ros::Rate loop_rate(10);
00447
00448 int count=0;
00449
00450
00451
00452
00453 while( ros::ok()){
00454
00455
00456 if (count == 10) {
00457
00458
00459
00460
00461
00462 }
00463
00464
00465 ++count;
00466 ros::spinOnce();
00467 loop_rate.sleep();
00468
00469 }
00470
00471
00472
00473 ROS_INFO("DONE");
00474
00475 }