ENH: search ball behavior

Projet-Navigation-UPS · Jan 10, 2017 · d84bce5 · d84bce5
1 parent 68146c8
commit d84bce5
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 37 deletions.
diff --git a/turtlebot_navigation/include/BallSearch.hpp b/turtlebot_navigation/include/BallSearch.hpp
@@ -21,6 +21,8 @@ class BallSearch
     //Messages
     std_msgs::Bool command_busy;
 
+    int etat_recherche;
+
 
 public:
 

diff --git a/turtlebot_navigation/src/BallSearch.cpp b/turtlebot_navigation/src/BallSearch.cpp
@@ -10,6 +10,7 @@ BallSearch::BallSearch(ros::NodeHandle& node):
     publisherBallReference(node.advertise<recherche_balle_tp1::command>("/nav/ball_reference", 1))
 {
     command_busy.data = true;
+    etat_recherche = 0;
 }
 
 BallSearch::~BallSearch(){}
@@ -103,6 +104,53 @@ Objet * BallSearch::Recherche_balle(unsigned char* raw, int  width, int height,
       if ( num_obj == -1 ) 
       {
          ROS_INFO("\nPas de balle visible");
+
+         if (!command_busy.data)
+         {
+
+         switch (etat_recherche)
+            {
+                case 0:
+                    ROS_INFO("On tourne à gauche de Pi/3 \n");
+                    this->sendBallReference(0, 1.5, 0, PI/3);
+                    etat_recherche = 1;
+                    break;
+                case 1:
+                    ROS_INFO("On tourne à droite de 2Pi/3 \n");
+                    this->sendBallReference(0, -1.5, 0, 2*PI/3);
+                    etat_recherche = 2;
+                    break;
+                case 2:
+                    ROS_INFO("On tourne à gauche de Pi \n");
+                    this->sendBallReference(0, 1.5, 0, PI);
+                    etat_recherche = 3;
+                    break;
+                case 3:
+                    ROS_INFO("On tourne à droite de 4Pi/3 \n");
+                    this->sendBallReference(0, -1.5, 0, 4*PI/3);
+                    etat_recherche = 4;
+                    break;
+                case 4:
+                    ROS_INFO("On tourne à droite de 5Pi/3 \n");
+                    this->sendBallReference(0, 1.5, 0, 5*PI/3);
+                    etat_recherche = 5;
+                    break;
+                case 5:
+                    ROS_INFO("On tourne à droite de 2Pi \n");
+                    this->sendBallReference(0, -1.5, 0, 2*PI);
+                    etat_recherche = 6;
+                    break;
+                case 6:
+                    ROS_INFO("Abandon recherche... \n");
+                    etat_recherche = 7;
+                    ros::shutdown();
+                    break;
+                default:
+                    ROS_INFO("Abandon recherche... \n");
+                    break;
+
+            }
+         }
       }
       else {         
          ROS_INFO("Numéro objet intéressant : %d \n", num_obj);
@@ -135,6 +183,8 @@ Objet * BallSearch::Recherche_balle(unsigned char* raw, int  width, int height,
          //printf("Barycentre : (%d, %d), Wmin, Wmax : (%d,%d), Hmin, Hmax : (%d,%d), Bounding box : (%d,%d), distance : %.2lf, Surface : %d \n", obj->Vcg, obj->Ucg,obj->Wmin,obj->Wmax, obj->Hmin, obj->Hmax, obj->Hmax-obj->Hmin, obj->Wmax-obj->Wmin, z, obj->Surface); 
          obj->Dist = z;
          obj->Theta = theta*180.0/PI;
+
+         etat_recherche=0;
       }
 
    return obj;

diff --git a/turtlebot_navigation/src/ballSearch_node.cpp b/turtlebot_navigation/src/ballSearch_node.cpp
@@ -15,7 +15,7 @@ int main(int argc, char **argv)
     ROS_INFO("Launching ballSearch_node ...\n");
     ros::init(argc, argv, "ballSearch_node");
     ros::NodeHandle n;
-    ros::Rate loop_rate(0.5); // 0.5Hz 
+    ros::Rate loop_rate(0.3); // 0.3Hz 
 
     BallSearch ballSearch(n);
     TurtleBotCamera turtleBotCamera(n);
@@ -27,8 +27,6 @@ int main(int argc, char **argv)
     unsigned char* raw;    
     unsigned char* rawFiltrageImage = new unsigned char[sizeof(unsigned char) * CAMERA_HEIGHT*(CAMERA_STEP_MONO)];
 
-    int etat_recherche=0;
-    int ES=0;
 
 
     ROS_INFO("Initiating ...\n");
@@ -52,41 +50,7 @@ int main(int argc, char **argv)
 
          if ( obj == NULL ) 
          {
-
             ROS_INFO("Pas de balle trouvée. \n");
-            switch (etat_recherche)
-            {
-                case 0:
-                    ROS_INFO("On tourne à gauche de Pi/4 \n");
-                    ballSearch.sendBallReference(0, 1.5, 0, PI/4);
-                    etat_recherche = 1;
-                    break;
-                case 1:
-                    ROS_INFO("On tourne à droite de Pi/2 \n");
-                    ballSearch.sendBallReference(0, -1.5, 0, PI/2);
-                    etat_recherche = 2;
-                    break;
-                case 2:
-                    ROS_INFO("On tourne à gauche de 3Pi/4 \n");
-                    ballSearch.sendBallReference(0, 1.5, 0, 3*PI/4);
-                    etat_recherche = 3;
-                    break;
-                case 3:
-                    ROS_INFO("On tourne à droite de Pi \n");
-                    ballSearch.sendBallReference(0, -1.5, 0, PI);
-                    etat_recherche = 4;
-                    break;
-                case 4:
-                    ROS_INFO("Abandon recherche... \n");
-                    etat_recherche = 5;
-                    ros::shutdown();
-                    break;
-                default:
-                    ROS_INFO("Abandon recherche... \n");
-                    break;
-
-}
-
          }
          else 
          {
-Original file line number
+Diff line change
@@ Expand Up / @@ -21,6 +21,8 @@ class BallSearch @@
         //Messages
         std_msgs::Bool command_busy;
+        int etat_recherche;
     public:
@@ Expand Down @@