更新走迷宫代码

src/infrared_maze/src/infrared_maze.cpp

@@ -13,9 +13,10 @@
 #include <std_msgs/Float32.h>
 
 
-#define  TURN_LEFT    0
-#define  TURN_RIGHT   1
-#define  TURN_BACK    2
+#define  GO_FORWARD   0
+#define  TURN_LEFT    1
+#define  TURN_RIGHT   2
+#define  TURN_BACK    3
 
 //global variable
 ros::Publisher twist_pub;
@@ -34,6 +35,10 @@ float linear_x_speed = 0.17;
 float linear_y_speed = 0.15;
 float angular_speed = 0.5;
 
+float now_yaw_data = 0.0;
+float start_yaw_data = 0.0;
+int turnFlag = 0;
+
 /*********************************************************
  * Descripition: 发布控制移动的消息,这里对于平面移动的
  *   机器人只需要控制linear_x,linear_y和angular_z即可.
@@ -133,16 +138,19 @@ int controlTurn(int flag)
     switch(flag)
     {
         case TURN_LEFT: //向左转动90°
+            turnFlag = TURN_LEFT;
             angular = angular_speed;
             check_angle = M_PI/2.0;
             break;
 
         case TURN_RIGHT: //向右转动90°
+            turnFlag = TURN_RIGHT;
             angular = -angular_speed;
             check_angle = M_PI/2.0;
             break;
 
         case TURN_BACK: //小车原地掉头
+            turnFlag = TURN_BACK;
             check_angle = M_PI;
             angular = angular_speed;
             break;
@@ -214,7 +222,8 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         ROS_WARN("Ready turn Left !");
         if(cnt >= 7)
         {
-            ROS_WARN("turn Left !");
+            start_yaw_data = now_yaw_data;
+            ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
             controlTurn(TURN_LEFT);
             flag = 1;
             cnt = 0;
@@ -233,6 +242,7 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         horizMoveMiddle(infrared_l, infrared_r, tolerance);
         publishTwistCmd(linear_x_speed, 0, 0); //go forward
         flag = 0;
+        turnFlag = GO_FORWARD;
     }
     else if((infrared_r == (float)0.3)&&(!flag))
     {
@@ -240,7 +250,8 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         ROS_WARN("Ready turn Right !");
         if(cnt >= 7)
         {
-            ROS_WARN("turn Right !");
+            start_yaw_data = now_yaw_data;
+            ROS_WARN("turn Right!, yaw_data:%f", start_yaw_data);
             controlTurn(TURN_RIGHT);
             flag = 1;
             cnt = 0;
@@ -255,7 +266,8 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
     }
     else
     {
-        ROS_WARN("turn Back !");
+        start_yaw_data = now_yaw_data;
+        ROS_WARN("turn Back! yaw_data:%f", start_yaw_data);
         controlTurn(TURN_BACK);
     }
 }
@@ -263,6 +275,74 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
 void yaw_callback(const std_msgs::Float32::ConstPtr& msg)
 {
     ROS_INFO("Yaw data:%f", msg->data);
+    now_yaw_data = msg->data;
+
+    if(turnFlag != GO_FORWARD) //开始纠正转弯的角度
+    {
+        float target = 0.0;
+        float diff = 0.0;
+        float sleep_time = 0.0;
+        float angular = 0.0;
+
+        if(turnFlag == TURN_LEFT)
+        {
+            target = start_yaw_data + M_PI/2.0;
+            if(target > M_PI)
+            {
+                target = target - M_PI*2.0;
+            }
+            ROS_INFO("Target Yaw: %f", target);
+            diff = target - now_yaw_data;
+            sleep_time = fabs(diff)/angular_speed;
+            if(diff > 0)
+            {
+                angular = -angular_speed;
+            }
+            else
+            {
+                angular = angular_speed;
+            }
+        }
+        else if(turnFlag == TURN_RIGHT)
+        {
+            target = start_yaw_data - M_PI/2.0;
+            if(target < -M_PI)
+            {
+                target = target + M_PI*2.0;
+            }
+            ROS_INFO("Target Yaw: %f", target);
+            diff = target - now_yaw_data;
+            sleep_time = fabs(diff)/angular_speed;
+            if(diff > 0)
+            {
+                angular = angular_speed;
+            }
+            else
+            {
+                angular = -angular_speed;
+            }
+        }
+        else //turn back
+        {
+            target = start_yaw_data + M_PI;
+            ROS_INFO("Target Yaw: %f", target);
+            diff = target - now_yaw_data;
+            sleep_time = fabs(diff)/angular_speed;
+            if(diff > 0)
+            {
+                angular = angular_speed;
+            }
+            else
+            {
+                angular = -angular_speed;
+            }
+        }
+        ROS_WARN("Yaw diff: %f, sleep:%f", diff, sleep_time*2.0);
+        publishTwistCmd(0, 0, angular);
+        ros::Duration(sleep_time*2.0).sleep();
+        ROS_WARN("Correct Yaw diff over !");
+        publishTwistCmd(0, 0, 0);
+    }
 }
 
 /*****************************************************************************