初步调试红外测距走迷宫正常

src/infrared_maze/src/infrared_maze.cpp

@@ -12,20 +12,6 @@
 #include <nav_msgs/Odometry.h>
 
 
-#define  setbit(x, y)  x|=(1<<y)
-#define  clrbit(x, y)  x&=~(1<<y)
-
-//low three bit as sonar warn flag
-//           left  font right
-// x x x x  x  0    0    0
-#define  STATUS_A   0x04  // v x x
-#define  STATUS_B   0x02  // x v x
-#define  STATUS_C   0x01  // x x v
-#define  STATUS_D   0x07  // v v v
-#define  STATUS_E   0x06  // v v x
-#define  STATUS_F   0x03  // x v v
-#define  STATUS_G   0x05  // v x v
-
 #define  TURN_LEFT    0
 #define  TURN_RIGHT   1
 #define  TURN_BACK    2
@@ -165,7 +151,7 @@ int controlTurn(int flag)
         last_angle = rotation;
         //ROS_INFO("turn_angle:%f",turn_angle);
     }
-    ros::Duration(0.1).sleep(); //100 ms
+    ros::Duration(0.2).sleep(); //200 ms
     ROS_INFO("Turning finish !!!");
 
     return 0;
@@ -177,40 +163,8 @@ int controlTurn(int flag)
 void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         float tolerance)
 {
-   unsigned char flag = 0;
-
-   if(infrared_l < warn_dist) //左边距离小于警告值
-   {
-       setbit(flag, 2);
-   }
-   else
-   {
-       clrbit(flag, 2);
-   }
-
-   if(infrared_r < warn_dist) //右边距离小于警告值
-   {
-       setbit(flag, 0);
-   }
-   else
-   {
-       clrbit(flag, 0);
-   }
-
-   if(infrared_f < warn_dist*2.0) //前边距离小于警告值
-   {
-       setbit(flag, 1);
-   }
-   else
-   {
-       clrbit(flag, 1);
-   }
-
-   ROS_INFO("CheckInfraredDist get status:0x%x", flag);
-   switch(flag)
-   {
-    case STATUS_B: //0x02
-        ROS_WARN("front warn, left right ok, compare left right dist !");
+    if(infrared_f < warn_dist*2.0) //前边距离小于警告值
+    {
         if(fabs(infrared_l - infrared_r) > tolerance)
         {
             if(infrared_l > infrared_r)
@@ -229,30 +183,14 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
             ROS_WARN("turn back !");
             controlTurn(TURN_BACK);
         }
-        break;
-
-    case STATUS_D: //0x07
-        ROS_WARN("left front right all warn, turn back !");
-        controlTurn(TURN_BACK);
-        break;
-
-    case STATUS_E: //
-        ROS_WARN("left front warn, right ok, turn right !");
-        controlTurn(TURN_RIGHT);
-        break;
-
-    case STATUS_F: // 0x03
-        ROS_WARN("left ok, front right warn, turn left");
-        controlTurn(TURN_LEFT);
-        break;
-
-    default: //go forward straight line - default
-        //始终控制小车保持在过道的中间位置
+    }
+    else
+    {
+       //始终控制小车保持在过道的中间位置
         horizontallyMoveMiddle(infrared_l, infrared_r, tolerance);
         ROS_INFO("Go forward, linear_x:%f", linear_x_speed);
         publishTwistCmd(linear_x_speed, 0, 0);
-        break;
-   }
+    }
 }
 
 /*****************************************************************************
@@ -289,17 +227,8 @@ int main(int argc, char **argv)
     ros::param::get("~odom_frame", odomFrame);
     ros::param::get("~base_frame", baseFrame);
 
-    ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 20, infrared_callback);
-    twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 10);
-
-#if 0
-    controlTurn(TURN_LEFT);
-    ros::Duration(5).sleep();
-    controlTurn(TURN_RIGHT);
-    ros::Duration(5).sleep();
-    controlTurn(TURN_BACK);
-    ros::Duration(5).sleep();
-#endif
+    ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
+    twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
 
     ros::spin();
     return 0;