更新使用红外测距传感器走迷宫代码,增加linear_x,y,tolerance参数

src/infrared_maze/cfg/param.yaml

@@ -8,7 +8,10 @@
 ######################################################
 infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
 cmd_topic: /cmd_vel
-linear_speed: 0.15
+linear_x: 0.18
+linear_y: 0.18
 angular_speed: 0.4
+warn_dist: 0.15
+tolerance_dist: 0.04
 odom_frame: /odom_combined
 base_frame: /base_footprint

src/infrared_maze/src/infrared_maze.cpp

@@ -26,35 +26,36 @@
 #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
+#define  TURN_LEFT    0
+#define  TURN_RIGHT   1
+#define  TURN_BACK    2
 
 //global variable
 geometry_msgs::Twist twist_cmd;
 ros::Publisher twist_pub;
 
-const double warn_dist = 0.15;  //warn check distance
+float warn_dist = 0.15;  //warn check distance
+float tolerance_dist = 0.04; //对警告距离的容忍值
 
-double default_period_hz = 10;  //hz
-double default_linear_x = 0.15;  // (m/s)
-double default_yaw_rate = 0.4;  // rad/s
-
-double front_dist;
-double left_dist;
-double right_dist;
+float front_dist;
+float left_dist;
+float right_dist;
 
 std::string odomFrame;
 std::string baseFrame;
 
-float linear_speed;
+float linear_x_speed;
+float linear_y_speed;
 float angular_speed;
 
-
-void publishTwistCmd(double linear_x, double angular_z)
+/*********************************************************
+ * Descripition: 发布控制移动的消息,这里对于平面移动的
+ *   机器人只需要控制linear_x,linear_y和angular_z即可.
+ ********************************************************/
+void publishTwistCmd(float linear_x, float linear_y, float angular_z)
 {
     twist_cmd.linear.x = linear_x;
-    twist_cmd.linear.y = 0.0;
+    twist_cmd.linear.y = linear_y;
     twist_cmd.linear.z = 0.0;
 
     twist_cmd.angular.x = 0.0;
@@ -64,7 +65,30 @@ void publishTwistCmd(double linear_x, double angular_z)
     twist_pub.publish(twist_cmd);
 }
 
-void controlTurn(int flag)
+/**********************************************************
+ * Description:控制小车移动在过道中间,比较左右两边的距离后
+ *   然后进行水平左右移动.
+ **********************************************************/
+void horizontallyMoveMiddle(float left_dis,float right_dis,float tolerance)
+{
+    int diff = left_dis - right_dis;
+    if(abs(diff) > tolerance)
+    {
+        if(diff < 0) //move right
+        {
+            publishTwistCmd(0, -linear_y_speed, 0);
+        }
+        else //move left
+        {
+            publishTwistCmd(0, linear_y_speed, 0);
+        }
+    }
+}
+
+/**************************************************
+ * Description:控制小车左右转90度,或者掉头180度.
+ *************************************************/
+int controlTurn(int flag)
 {
     tf::TransformListener tf_Listener;
     tf::StampedTransform tf_Transform;
@@ -73,23 +97,24 @@ void controlTurn(int flag)
 
     switch(flag)
     {
-        case TURN_LEFT:
+        case TURN_LEFT: //向左转动90°
             angular = angular_speed;
             check_angle = M_PI/2;
             break;
 
-        case TURN_RIGHT:
+        case TURN_RIGHT: //向右转动90°
             angular = -angular_speed;
             check_angle = M_PI/2;
             break;
 
-        case TURN_BACK:
+        case TURN_BACK: //小车原地掉头
             check_angle = M_PI;
             angular = angular_speed;
             break;
 
         default:
-            break;
+            ROS_ERROR("Turn flag error !");
+            return -1;
     }
 
     try
@@ -98,16 +123,18 @@ void controlTurn(int flag)
     }
     catch(tf::TransformException &ex)
     {
-        ROS_ERROR("waitForTransform timeout error !");
+        ROS_ERROR("tf_Listener.waitForTransform timeout error !");
         ros::shutdown();
+        return -2;
     }
 
+    ROS_INFO("Check_angle:%d, angular_speed:%d",check_angle, angular);
     double last_angle = fabs(tf::getYaw(tf_Transform.getRotation()));
     double turn_angle = 0;
     while((fabs(turn_angle) < check_angle)&&(ros::ok()))
     {
         ROS_INFO("Turning ...");
-        publishTwistCmd(0, angular);
+        publishTwistCmd(0, 0, angular); //原地转弯,不需要linear_x,y的控制
         ros::Duration(0.05).sleep(); //50 ms
 
         tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
@@ -117,13 +144,21 @@ void controlTurn(int flag)
         turn_angle += delta_angle;
         last_angle = rotation;
     }
+
+    return 0;
 }
 
-void checkInfraredDist(double infrared_f, double infrared_l, double infrared_r)
+/***************************************************************************
+ * Description:检查三个传感器数据,然后控制小车移动.
+ **************************************************************************/
+void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r)
 {
    unsigned char flag = 0;
 
-   if(infrared_l < warn_dist)
+   //始终控制小车保持在过道的中间位置
+   horizontallyMoveMiddle(infrared_l, infrared_r, tolerance_dist);
+
+   if(infrared_l < warn_dist) //左边距离小于警告值
    {
        setbit(flag, 2);
    }
@@ -132,96 +167,75 @@ void checkInfraredDist(double infrared_f, double infrared_l, double infrared_r)
        clrbit(flag, 2);
    }
 
-   if(infrared_f < warn_dist)
+   if(infrared_r < warn_dist) //右边距离小于警告值
    {
-       setbit(flag, 1);
+       setbit(flag, 0);
    }
    else
    {
-       clrbit(flag, 1);
+       clrbit(flag, 0);
    }
 
-   if(infrared_r < warn_dist)
+   if(infrared_f < warn_dist) //前边距离小于警告值
    {
-       setbit(flag, 0);
+       setbit(flag, 1);
    }
    else
    {
-       clrbit(flag, 0);
+       clrbit(flag, 1);
    }
 
    ROS_INFO("CheckInfraredDist get status:0x%x", flag);
    switch(flag)
    {
-    case STATUS_A: //turn right 0x04
-        ROS_WARN("left warn,turn right");
-        //publishTwistCmd(0, -default_yaw_rate);
-        controlTurn(TURN_RIGHT);
-        break;
-
-    case STATUS_B: // 0x02
-        ROS_WARN("front warn, left and right ok, compare left and right value to turn");
+    case STATUS_B: //0x02
+        ROS_WARN("front warn, left right ok, compare left right value !");
         if(infrared_l > infrared_r)
         {
             ROS_WARN("turn left");
             controlTurn(TURN_LEFT);
-            //publishTwistCmd(0, 2*default_yaw_rate);
         }
         else
         {
             ROS_WARN("turn right");
             controlTurn(TURN_RIGHT);
-            //publishTwistCmd(0, -default_yaw_rate*2);
         }
         break;
 
-    case STATUS_C: //turn left
-        ROS_WARN("left ok, front ok, right warn, turn left");
-        //publishTwistCmd(0, default_yaw_rate);
-        controlTurn(TURN_LEFT);
-        break;
-
-    case STATUS_D:
-        ROS_WARN("left,front,right all warn, turn back");
+    case STATUS_D: //0x07
+        ROS_WARN("left front right all warn, turn back !");
         controlTurn(TURN_BACK);
-        //publishTwistCmd(0, 10*default_yaw_rate);
         break;
 
-    case STATUS_E:
-        ROS_WARN("left warn, front warn, right ok, turn right");
+    case STATUS_E: //
+        ROS_WARN("left front warn, right ok, turn right !");
         controlTurn(TURN_RIGHT);
-        //publishTwistCmd(0, (-default_yaw_rate*2));
         break;
 
-    case STATUS_F:
-        ROS_WARN("left ok, front warn, right warn, turn left");
+    case STATUS_F: // 0x03
+        ROS_WARN("left ok, front right warn, turn left");
         controlTurn(TURN_LEFT);
-        //publishTwistCmd(0, (default_yaw_rate*2));
         break;
 
-    case STATUS_G:
-        ROS_WARN("left and right warn, front ok, speed up");
-        publishTwistCmd(default_linear_x, 0);
-        //publishTwistCmd(2*default_linear_x, 0);
-        break;
-
-    default: //go forward straight line
-        publishTwistCmd(default_linear_x, 0);
+    default: //go forward straight line - default
+        publishTwistCmd(linear_x_speed, 0, 0);
         break;
    }
 }
 
+/*****************************************************************************
+ * Description: 订阅红外测距传感器话题后的回调函数,根据三个传感器数据进行走
+ *   迷宫.
+ ****************************************************************************/
 void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
 {
-    ROS_INFO("front distance:[%f]", msg->front_dist);
+    ROS_INFO("front left right distance:[%f %f %f]", msg->front_dist,
+            msg->left_dist, msg->right_dist);
     front_dist = msg->front_dist;
-
-    ROS_INFO("left distance:[%f]", msg->left_dist);
-    left_dist = msg->left_dist;
-
-    ROS_INFO("right distance:[%f]", msg->right_dist);
+    left_dist  = msg->left_dist;
     right_dist = msg->right_dist;
 
+    //根据三个传感器反馈的测距值开始移动
     checkInfraredDist(front_dist, left_dist, right_dist);
 }
 
@@ -234,9 +248,12 @@ int main(int argc, char **argv)
     ros::NodeHandle handle;
 
     ros::param::get("~infrared_topic", infrared_topic);
-    ros::param::get("~cmd_topic",  cmd_topic);
-    ros::param::get("~linear_speed", linear_speed);
+    ros::param::get("~cmd_topic", cmd_topic);
+    ros::param::get("~linear_x", linear_x_speed);
+    ros::param::get("~linear_y", linear_y_speed);
     ros::param::get("~angular_speed", angular_speed);
+    ros::param::get("~warn_dist", warn_dist);
+    ros::param::get("~tolerance_dist", tolerance_dist);
     ros::param::get("~odom_frame", odomFrame);
     ros::param::get("~base_frame", baseFrame);