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 4
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 printf("START MOVE()\n");
00164 printf("Goal distance %f\n", distance);
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 printf("Actual distance %f\n", calDist);
00186 }
00187
00188
00189 speedM.linear.x= 0;
00190 cmd_velocidade.publish(speedM);
00191 ros::spinOnce();
00192
00193 printf("FINISH MOVE()\n");
00194 }
00195
00196
00197
00198 void rotate(float angle_deg, float speed){
00199
00200 printf("START ROTATE()\n");
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 printf("Rotate to right.");
00216 } else {
00217 angle_deg=angle_deg-(rotateSpeed*10);
00218 speedR.angular.z= speed;
00219 direction = 1;
00220 printf("Rotate to left.");
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 printf("Actual angle %f\n", ang);
00250 printf("final ang %f\n", finalAng);
00251
00252 if (ang < 0 ) {
00253
00254 while (ang < finalAng){
00255 cmd_velocidade.publish(speedR);
00256 ros::spinOnce();
00257 ang= quaternion2degrees( theta , thetaPol);
00258 printf("Actual angle %f\n", ang);
00259 printf("final ang %f\n", finalAng);
00260 }
00261 break;
00262 }
00263 }
00264
00265 } else {
00266
00267 while( ang < finalAng ){
00268 cmd_velocidade.publish(speedR);
00269 ros::spinOnce();
00270 ang= quaternion2degrees( theta , thetaPol);
00271 printf("Actual angle %f\n", ang);
00272 printf("final ang %f\n", finalAng);
00273 }
00274 }
00275
00276 } else {
00277
00279
00280 if ( ang < 0 && finalAng > 0 ) {
00281
00282 while (ang < finalAng){
00283 cmd_velocidade.publish(speedR);
00284 ros::spinOnce();
00285 ang= quaternion2degrees( theta , thetaPol);
00286 printf("Actual angle %f\n", ang);
00287 printf("final ang %f\n", finalAng);
00288 }
00289
00290 } else {
00291
00292 while( ang > finalAng ){
00293 cmd_velocidade.publish(speedR);
00294 ros::spinOnce();
00295 ang= quaternion2degrees( theta , thetaPol);
00296 printf("Actual angle %f\n", ang);
00297 printf("final ang %f\n", finalAng);
00298 }
00299 }
00300
00301 }
00302
00303
00304 speedR.angular.z= 0;
00305 cmd_velocidade.publish(speedR);
00306 ros::spinOnce();
00307
00308 printf("FINISH ROTATE()\n");
00309 }
00310
00311
00312 void reactiveNavigation() {
00313
00314 int cnt=0;
00315
00316 geometry_msgs::Twist speedM;
00317
00318 speedM.linear.x= moveSpeed ;
00319
00320 while (cnt < 1000) {
00321
00322 usleep(100000);
00323
00324 printf("Moving....\n");
00325 cmd_velocidade.publish(speedM);
00326 ros::spinOnce();
00327
00328 while (rightSonar <= reactiveRange || leftSonar<=reactiveRange || centerSonar<=reactiveRange) {
00329
00330 usleep(100000);
00331
00332
00333
00334
00335
00336 if (rightSonar <= leftSonar) {
00337
00338 while (centerSonar<=reactiveRange || rightSonar<=reactiveRange) {
00339 usleep(100000);
00340 rotate(10,rotateSpeed);
00341
00342 }
00343
00344
00345 } else {
00346
00347 while (centerSonar<=reactiveRange || leftSonar<=reactiveRange) {
00348 usleep(100000);
00349 rotate(-10,rotateSpeed);
00350 }
00351 }
00352
00353 printf("STOP reaction....\n");
00354 }
00355
00356
00357
00358 ++cnt;
00359
00360 }
00361
00362
00363 }
00364
00365
00366
00367
00368 void goTo(float xIni, float yIni, float teta, float xFinal, float yFinal, float tetaFinal){
00369
00370
00371 float dist=0, alfa=0, phi=0;
00372
00373 printf("START GO TO()\n");
00374
00375
00376
00377 if ( xIni==xFinal && yIni==yFinal) {
00378
00379 phi = tetaFinal - teta;
00380 rotate(phi,rotateSpeed);
00381
00382
00383 } else {
00384
00385 dist=sqrt(pow(xFinal-xIni,2)+pow(yFinal-yIni,2));
00386 alfa=atan2((yFinal-yIni),(xFinal-xIni))*180.0/PI;
00387 phi=alfa-teta;
00388
00389
00390
00391
00392 if (abs(phi)>180)
00393 {
00394 phi=phi-(abs(phi)/phi)*360.0;
00395 }
00396
00397 printf("PHI ROTATION: %f \n", phi);
00398 rotate(phi,rotateSpeed);
00399
00400
00401 printf("LINEAR MOVE: %f \n", dist);
00402 move(dist,moveSpeed);
00403
00404 }
00405
00406
00407
00408 x_goTo = xFinal;
00409 y_goTo = yFinal;
00410 theta_goTo = theta_goTo + phi;
00411
00412
00413 printf("PHI FINAL: %f \n", theta_goTo);
00414 printf("FINISH GO TO()\n");
00415 }
00416
00417
00418 int main(int argc, char** argv)
00419 {
00420 ros::init(argc, argv, "Virtual_TraxBot_Stage");
00421 ros::NodeHandle nh;
00422
00423 ROS_INFO("ROS in TraxBot: Start ROS move in stage...");
00424
00425
00426
00427 sonarsRange_stage = nh.subscribe("/base_scan", 1, sonars_readCB);
00428 odm_stage = nh.subscribe("/odom", 1, odom_readCB);
00429
00430
00431 cmd_velocidade = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
00432
00433
00434 ros::Rate loop_rate(10);
00435
00436 int count=0;
00437
00438
00439
00440 while( ros::ok()){
00441
00442
00443 if (count == 10) {
00444
00445 reactiveNavigation();
00446
00447
00448
00449 }
00450
00451 printf("count: %d\n",count);
00452 ++count;
00453 ros::spinOnce();
00454 loop_rate.sleep();
00455
00456 }
00457
00458
00459
00460 ROS_INFO("DONE");
00461
00462 }