更新红外测据走迷宫代码,调试好转向90和掉头功能

src/infrared_maze/cfg/param.yaml

@@ -9,9 +9,9 @@
 infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
 cmd_topic: /cmd_vel
 linear_x: 0.18
-linear_y: 0.18
+linear_y: 0.16
 angular_speed: 0.4
-warn_dist: 0.15
-tolerance_dist: 0.04
+warn_dist: 0.12
+tolerance_dist: 0.05
 odom_frame: /odom_combined
 base_frame: /base_footprint

src/infrared_maze/src/infrared_maze.cpp

@@ -31,7 +31,6 @@
 #define  TURN_BACK    2
 
 //global variable
-geometry_msgs::Twist twist_cmd;
 ros::Publisher twist_pub;
 
 float warn_dist = 0.15;  //warn check distance
@@ -44,9 +43,9 @@ float right_dist;
 std::string odomFrame;
 std::string baseFrame;
 
-float linear_x_speed;
-float linear_y_speed;
-float angular_speed;
+float linear_x_speed = 0.18;
+float linear_y_speed = 0.16;
+float angular_speed = 0.4;
 
 /*********************************************************
  * Descripition: 发布控制移动的消息,这里对于平面移动的
@@ -54,15 +53,17 @@ float angular_speed;
  ********************************************************/
 void publishTwistCmd(float linear_x, float linear_y, float angular_z)
 {
-    twist_cmd.linear.x = linear_x;
-    twist_cmd.linear.y = linear_y;
-    twist_cmd.linear.z = 0.0;
+    geometry_msgs::Twist twist_msg;
 
-    twist_cmd.angular.x = 0.0;
-    twist_cmd.angular.y = 0.0;
-    twist_cmd.angular.z = angular_z;
+    twist_msg.linear.x = linear_x;
+    twist_msg.linear.y = linear_y;
+    twist_msg.linear.z = 0.0;
 
-    twist_pub.publish(twist_cmd);
+    twist_msg.angular.x = 0.0;
+    twist_msg.angular.y = 0.0;
+    twist_msg.angular.z = angular_z;
+
+    twist_pub.publish(twist_msg);
 }
 
 /**********************************************************
@@ -71,17 +72,25 @@ void publishTwistCmd(float linear_x, float linear_y, float angular_z)
  **********************************************************/
 void horizontallyMoveMiddle(float left_dis,float right_dis,float tolerance)
 {
-    int diff = left_dis - right_dis;
-    if(abs(diff) > tolerance)
+    float diff = left_dis - right_dis;
+    //ROS_INFO("left right diff:%f", diff);
+    if(fabs(diff) > tolerance)
     {
         if(diff < 0) //move right
         {
+            ROS_INFO("move right horizontally !");
             publishTwistCmd(0, -linear_y_speed, 0);
         }
         else //move left
         {
+            ROS_INFO("move left horizontally !");
             publishTwistCmd(0, linear_y_speed, 0);
         }
+        ros::Duration(0.1).sleep(); //100 ms
+    }
+    else //stop move
+    {
+        publishTwistCmd(0, 0, 0);
     }
 }
 
@@ -92,19 +101,19 @@ int controlTurn(int flag)
 {
     tf::TransformListener tf_Listener;
     tf::StampedTransform tf_Transform;
-    int angular = 0;
-    int check_angle = 0;
+    float angular = 0.0;
+    float check_angle = 0.0;
 
     switch(flag)
     {
         case TURN_LEFT: //向左转动90°
             angular = angular_speed;
-            check_angle = M_PI/2;
+            check_angle = M_PI/2.0;
             break;
 
         case TURN_RIGHT: //向右转动90°
             angular = -angular_speed;
-            check_angle = M_PI/2;
+            check_angle = M_PI/2.0;
             break;
 
         case TURN_BACK: //小车原地掉头
@@ -119,31 +128,45 @@ int controlTurn(int flag)
 
     try
     {
-        tf_Listener.waitForTransform(odomFrame, baseFrame, ros::Time(), ros::Duration(2.0));
+        tf_Listener.waitForTransform(odomFrame, baseFrame, ros::Time(0), ros::Duration(5.0));
     }
     catch(tf::TransformException &ex)
     {
         ROS_ERROR("tf_Listener.waitForTransform timeout error !");
+        ROS_ERROR("%s",ex.what());
         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;
+    //ROS_INFO("Check_angle:%f, angular_speed:%f",check_angle, angular);
+    tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+    float last_angle = fabs(tf::getYaw(tf_Transform.getRotation()));
+    float turn_angle = 0.0;
+    //ROS_INFO("first_angle:%f",last_angle);
     while((fabs(turn_angle) < check_angle)&&(ros::ok()))
     {
         ROS_INFO("Turning ...");
         publishTwistCmd(0, 0, angular); //原地转弯,不需要linear_x,y的控制
-        ros::Duration(0.05).sleep(); //50 ms
+        ros::Duration(0.04).sleep(); //40 ms
 
-        tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
-        double rotation = fabs(tf::getYaw(tf_Transform.getRotation()));
-        double delta_angle = fabs(rotation - last_angle);
+        try
+        {
+            tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+        }
+        catch(tf::TransformException &ex)
+        {
+            ROS_ERROR("%s", ex.what());
+            continue;
+        }
+        float rotation = fabs(tf::getYaw(tf_Transform.getRotation()));
+        float delta_angle = fabs(rotation - last_angle);
 
         turn_angle += delta_angle;
         last_angle = rotation;
+        //ROS_INFO("turn_angle:%f",turn_angle);
     }
+    ros::Duration(0.1).sleep(); //100 ms
+    ROS_INFO("Turning finish !!!");
 
     return 0;
 }
@@ -151,13 +174,11 @@ int controlTurn(int flag)
 /***************************************************************************
  * Description:检查三个传感器数据,然后控制小车移动.
  **************************************************************************/
-void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r)
+void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
+        float tolerance)
 {
    unsigned char flag = 0;
 
-   //始终控制小车保持在过道的中间位置
-   horizontallyMoveMiddle(infrared_l, infrared_r, tolerance_dist);
-
    if(infrared_l < warn_dist) //左边距离小于警告值
    {
        setbit(flag, 2);
@@ -176,7 +197,7 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r)
        clrbit(flag, 0);
    }
 
-   if(infrared_f < warn_dist) //前边距离小于警告值
+   if(infrared_f < warn_dist*2.0) //前边距离小于警告值
    {
        setbit(flag, 1);
    }
@@ -189,16 +210,24 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r)
    switch(flag)
    {
     case STATUS_B: //0x02
-        ROS_WARN("front warn, left right ok, compare left right value !");
-        if(infrared_l > infrared_r)
+        ROS_WARN("front warn, left right ok, compare left right dist !");
+        if(fabs(infrared_l - infrared_r) > tolerance)
         {
-            ROS_WARN("turn left");
-            controlTurn(TURN_LEFT);
+            if(infrared_l > infrared_r)
+            {
+                ROS_WARN("turn left !");
+                controlTurn(TURN_LEFT);
+            }
+            else if(infrared_l < infrared_r)
+            {
+                ROS_WARN("turn right !");
+                controlTurn(TURN_RIGHT);
+            }
         }
         else
         {
-            ROS_WARN("turn right");
-            controlTurn(TURN_RIGHT);
+            ROS_WARN("turn back !");
+            controlTurn(TURN_BACK);
         }
         break;
 
@@ -218,6 +247,9 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r)
         break;
 
     default: //go forward straight line - default
+        //始终控制小车保持在过道的中间位置
+        horizontallyMoveMiddle(infrared_l, infrared_r, tolerance);
+        ROS_INFO("Go forward, linear_x:%f", linear_x_speed);
         publishTwistCmd(linear_x_speed, 0, 0);
         break;
    }
@@ -236,7 +268,7 @@ void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
     right_dist = msg->right_dist;
 
     //根据三个传感器反馈的测距值开始移动
-    checkInfraredDist(front_dist, left_dist, right_dist);
+    checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist);
 }
 
 int main(int argc, char **argv)
@@ -260,6 +292,15 @@ int main(int argc, char **argv)
     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::spin();
     return 0;
 }