更新走迷宫代码,当在平移的时候也不断的修正角度,防止角度偏了

src/infrared_maze/src/infrared_maze.cpp

@@ -1,6 +1,8 @@
-/**************************************************************
+/***************************************************************
  Copyright(C): 2016-2020 ROS小课堂 www.corvin.cn
- Description:三轮小车进行红外避障走迷宫.
+ Description:三轮小车进行红外避障走迷宫.根据前,左,右三个红外测距
+   信息来进行移动,这里的移动策略是每次移动0.5m,然后再判断距离.
+   移动时向左转优先,其次直行,最后是右转.
  Author: jally, corvin
  History:
     20200404:增加可以直接转90度,掉头的功能 by corvin.
@@ -20,6 +22,8 @@
 #define  TURN_LEFT    1
 #define  TURN_RIGHT   2
 #define  TURN_BACK    3
+#define  HORIZ_MOVE   4
+
 #define  ERROR       -1
 
 //global variable
@@ -68,31 +72,77 @@ void publishTwistCmd(float linear_x, float linear_y, float angular_z)
 }
 
 /**********************************************************
- * Description:控制小车移动在过道中间,比较左右两边的距离后
- *   然后进行水平左右移动.
+ * Description:调用IMU板发布出来的yaw信息,可以根据该数据
+ *   来修正转弯时不准确的问题.注意这里的yaw数据为弧度值.
+ *   注意往右旋转为负值,左旋转为正值.右手定则.
+ *   注意在ROS中IMU数据中yaw的范围,具体表示如下:
+ *                0  -0
+ *                -----
+ *              /      \
+ *         1.57|        | -1.57
+ *              \      /
+ *               ------
+ *             3.14 -3.14
  **********************************************************/
-void horizMoveMiddle(float left, float right, float tolerance)
+void yawUpdate(const float start_yaw)
 {
-    float diff = left - right;
-    ROS_INFO("diff:%f,left:%f,right:%f",diff, left, right);
-    if((fabs(diff) > tolerance)&&(left!=(float)0.3)&&(right!=(float)0.3))
+    float target = 0.0;
+    float diff = 10.0;
+    float angular = 0.0;
+    float now_yaw = 0.0;
+
+    //开始计算修正后的目标角度
+    if((turnFlag == GO_FORWARD)||(turnFlag == HORIZ_MOVE))
     {
-        if(diff < 0) //move right
+        target = start_yaw;
+    }
+    else if(turnFlag == TURN_LEFT)
+    {
+        target = start_yaw + M_PI/2.0;
+        if(target > M_PI)
         {
-            ROS_WARN("move Right horizontally !");
-            publishTwistCmd(0, -linear_y_speed, 0);
+            target = target - M_PI*2.0;
         }
-        else //move left
+    }
+    else if(turnFlag == TURN_RIGHT)
+    {
+        target = start_yaw - M_PI/2.0;
+        if(target < -M_PI)
         {
-            ROS_WARN("move Left horizontally !");
-            publishTwistCmd(0, linear_y_speed, 0);
+            target = target + M_PI*2.0;
+        }
+    }
+    else //turn back
+    {
+        target = start_yaw + M_PI;
+        if(target > M_PI)
+        {
+            target = target - M_PI*2.0;
         }
-        ros::Duration(0.2).sleep(); //delay 200 ms
     }
-    else //stop move
+
+    //开始不断的计算偏差,调整角度
+    while(fabs(diff) > 0.01)
     {
-        publishTwistCmd(0, 0, 0);
+        yawSrvClient.call(yawSrv);
+        now_yaw = yawSrv.response.yaw;
+        ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
+
+        diff = target - now_yaw;
+        if(diff > 0)
+        {
+            angular = angular_speed;
+        }
+        else
+        {
+            angular = -angular_speed;
+        }
+        ROS_WARN("Yaw diff:%f, angular:%f", diff, angular);
+        publishTwistCmd(0, 0, angular);
+        ros::Duration(0.04).sleep();
     }
+    publishTwistCmd(0, 0, 0); //在修正角度结束后应该立刻停止小车转动
+    ROS_INFO("Correct Yaw diff over !");
 }
 
 /****************************************************
@@ -123,7 +173,7 @@ void goForward()
 
     distSrvClient.call(distSrv); //通过服务调用来获取测距值
     front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
-    ROS_WARN("Front dist:%f", front_dist);
+    ROS_INFO("Front dist:%f", front_dist);
     while((dist<check_dist)&&(front_dist>tolerance_dist*3.0)&&(ros::ok()))
     {
         ROS_INFO("Go forward, dist:%f", dist);
@@ -137,7 +187,7 @@ void goForward()
 
         distSrvClient.call(distSrv); //通过服务调用来获取测距值
         front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
-        ROS_WARN("Front dist:%f", front_dist);
+        ROS_INFO("Front dist:%f", front_dist);
     }
     publishTwistCmd(0, 0, 0); //stop move
     ROS_INFO("Go forward finish !!!");
@@ -230,6 +280,56 @@ int controlTurn(int flag)
     return 0;
 }
 
+/**********************************************************
+ * Description:控制小车移动在过道中间,比较左右两边的距离后
+ *   然后进行水平左右移动.
+ **********************************************************/
+void horizMoveMiddle(float tolerance)
+{
+    float diff = 0.0;
+    float begin_yaw = 0.0;
+    float now_left = 0.0;
+    float now_right = 0.0;
+
+    turnFlag = HORIZ_MOVE;
+    yawSrvClient.call(yawSrv); //记录平移前的角度,作为标准进行修正
+    begin_yaw = yawSrv.response.yaw;
+
+    distSrvClient.call(distSrv);
+    now_left = distSrv.response.left_dist;
+    ros::Duration(0.02).sleep();//在获取两个传感器值之间需要加延迟
+    distSrvClient.call(distSrv);
+    now_right = distSrv.response.right_dist;
+
+    diff = now_left - now_right;
+    ROS_INFO("diff:%f,left:%f,right:%f",diff,now_left,now_right);
+    while((fabs(diff)>tolerance)&&(now_left!=(float)0.3)&&(now_right!=(float)0.3))
+    {
+        if(diff < 0) //move right
+        {
+            ROS_WARN("move Right horizontally !");
+            publishTwistCmd(0, -linear_y_speed, 0);
+        }
+        else //move left
+        {
+            ROS_WARN("move Left horizontally !");
+            publishTwistCmd(0, linear_y_speed, 0);
+        }
+        ros::Duration(0.1).sleep(); //delay 100ms
+        yawUpdate(begin_yaw); //用于修正转歪的角度
+
+        distSrvClient.call(distSrv);
+        now_left = distSrv.response.left_dist;
+        ros::Duration(0.02).sleep();//在获取两个传感器值之间需要加延迟
+        distSrvClient.call(distSrv);
+        now_right = distSrv.response.right_dist;
+
+        diff = now_left - now_right;
+    }
+    //当平移结束后立刻停止移动
+    publishTwistCmd(0, 0, 0); //stop move
+}
+
 /***************************************************************************
  * Description:检查三个传感器数据,然后控制小车移动.这里需要防止小车在原地
  *   进行连续两次旋转,在每次转弯后必须有个直行.这里需要注意的是当小车在转弯
@@ -255,7 +355,7 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
     {
         //始终控制小车保持在过道的中间位置
         ROS_WARN("go Forward ! yaw_data: %f", start_yaw_data);
-        horizMoveMiddle(infrared_l, infrared_r, tolerance);
+        horizMoveMiddle(tolerance);
         turnFlag = GO_FORWARD;
         flag = 0;
     }
@@ -274,80 +374,6 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
     controlTurn(turnFlag); //开始执行动作
 }
 
-/**********************************************************
- * Description:调用IMU板发布出来的yaw信息,可以根据该数据
- *   来修正转弯时不准确的问题.注意这里的yaw数据为弧度值.
- *   注意往右旋转为负值,左旋转为正值.右手定则.
- *   注意在ROS中IMU数据中yaw的范围,具体表示如下:
- *                0  -0
- *                -----
- *              /      \
- *         1.57|        | -1.57
- *              \      /
- *               ------
- *             3.14 -3.14
- **********************************************************/
-void yawUpdate(const float start_yaw)
-{
-    float target = 0.0;
-    float diff = 10.0;
-    float angular = 0.0;
-    float now_yaw = 0.0;
-
-    //开始计算修正后的目标角度
-    if(turnFlag == GO_FORWARD)
-    {
-        target = start_yaw;
-    }
-    else if(turnFlag == TURN_LEFT)
-    {
-        target = start_yaw + M_PI/2.0;
-        if(target > M_PI)
-        {
-            target = target - M_PI*2.0;
-        }
-    }
-    else if(turnFlag == TURN_RIGHT)
-    {
-        target = start_yaw - M_PI/2.0;
-        if(target < -M_PI)
-        {
-            target = target + M_PI*2.0;
-        }
-    }
-    else //turn back
-    {
-        target = start_yaw + M_PI;
-        if(target > M_PI)
-        {
-            target = target - M_PI*2.0;
-        }
-    }
-
-    //开始不断的计算偏差,调整角度
-    while(fabs(diff) > 0.01)
-    {
-        yawSrvClient.call(yawSrv);
-        now_yaw = yawSrv.response.yaw;
-        ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
-
-        diff = target - now_yaw;
-        if(diff > 0)
-        {
-            angular = angular_speed;
-        }
-        else
-        {
-            angular = -angular_speed;
-        }
-        ROS_WARN("Yaw diff:%f, angular:%f", diff, angular);
-        publishTwistCmd(0, 0, angular);
-        ros::Duration(0.04).sleep();
-    }
-    publishTwistCmd(0, 0, 0); //在修正角度结束后应该立刻停止小车转动
-    ROS_INFO("Correct Yaw diff over !");
-}
-
 /*****************************************************************************
  * Description: 订阅红外测距传感器话题后的回调函数,根据三个传感器数据进行走
  *   迷宫.
@@ -367,8 +393,8 @@ void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
 
 /************************************************************
  * Description:主函数初始化节点,读取配置参数,订阅和发布话题,
- *   在订阅的红外测距数据话题回调函数中进行移动,在订阅的yaw
- *   话题中进行转弯角度的修正.
+ *   在订阅的红外测距数据话题回调函数中进行移动,通过调用yaw
+ *   服务进行转弯角度的修正.
  ************************************************************/
 int main(int argc, char **argv)
 {