更新走迷宫代码,前进距离可以通过参数配置

src/CMakeLists.txt

@@ -1 +1 @@
-/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
+/opt/ros/kinetic/catkin/cmake/toplevel.cmake

src/infrared_maze/cfg/param.yaml

@@ -2,21 +2,34 @@
 # Copyright: 2016-2020 www.corvin.cn ROS小课堂
 # Description:使用小车上三个红外测距传感器进行避障
 #   走迷宫,这里是一些可以配置的参数.
+#   infrared_topic:订阅的红外传感器话题.
+#   cmd_topic:控制移动的话题名.
+#   yaw_service:发布IMU模块yaw偏行角的服务.
+#   infrared_service:发布红外测据信息的服务名.
+#   linear_x:移动时前进速度大小.
+#   linear_y:横向移动时大小.
+#   angular_speed:原地转圈时角速度大小.
+#   warn_dist:小车是否可以前进的距离.
+#   tolerance_dist:距离容忍值.
+#   odom_frame:odom的tf名称.
+#   base_frame:底盘的tf名称.
+#   forward_dist:每次前进时移动的距离.
 # Author: corvin
 # History:
 #   20200404:init this file.
 #   20200406:增加yaw service,rate参数.
 #   20200412:增加获取红外测距信息的service.
+#   20200424:更新注释,增加参数介绍.
 ######################################################
 infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
 cmd_topic: /cmd_vel
 yaw_service: /imu_node/GetYawData
-dist_service: /mobilebase_arduino/getInfraredDistance
+infrared_service: /mobilebase_arduino/getInfraredDistance
+odom_frame: /odom_combined
+base_frame: /base_footprint
 linear_x: 0.17
 linear_y: 0.15
 angular_speed: 0.5
-warn_dist: 0.15
+warn_dist: 0.25
 tolerance_dist: 0.05
-odom_frame: /odom_combined
-base_frame: /base_footprint
-rate: 1
+forward_dist: 0.50

src/infrared_maze/src/infrared_maze.cpp

@@ -7,6 +7,7 @@
  History:
     20200404:增加可以直接转90度,掉头的功能 by corvin.
     20200412:增加使用服务方式来获取红外测距值-corvin.
+    20200424:增加每次前进时移动距离的参数,可以配置.-corvin
  **************************************************************/
 #include <ros/ros.h>
 #include <std_msgs/Float32.h>
@@ -35,8 +36,9 @@ rasp_imu_hat_6dof::GetYawData yawSrv;
 ros::ServiceClient distSrvClient;
 ros_arduino_msgs::GetInfraredDistance distSrv;
 
-float warn_dist = 0.15;  //warn check distance
+float warn_dist = 0.25;  //warn check distance
 float tolerance_dist = 0.05; //对警告距离的容忍值
+float forward_dist = 0.50; //每次前进移动时走的距离
 
 float front_dist;
 float left_dist;
@@ -148,12 +150,11 @@ void yawUpdate(const float start_yaw)
 /****************************************************
  * Description:控制小车前进移动0.5m
  ***************************************************/
-void goForward()
+void goForward(const float check_dist)
 {
     tf::TransformListener tf_Listener;
     tf::StampedTransform tf_Transform;
-    float dist = 0.0;
-    float check_dist = 0.50;
+    float dist = 0.0; //每次前进移动的距离
     float front_dist = 0.0;
 
     try
@@ -191,7 +192,6 @@ void goForward()
     }
     publishTwistCmd(0, 0, 0); //stop move
     ROS_INFO("Go forward finish !!!");
-    ros::Duration(0.1).sleep(); //100 ms
 }
 
 /*************************************************************
@@ -226,7 +226,7 @@ int controlTurn(int flag)
             break;
 
         case GO_FORWARD: //小车直行50cm
-            goForward();
+            goForward(forward_dist);
             return 1;
 
         default:
@@ -282,13 +282,13 @@ int controlTurn(int flag)
 
 /**********************************************************
  * Description:控制小车移动在过道中间,比较左右两边的距离后
- *   然后进行水平左右移动.
+ *   然后进行水平左右移动.使小车保持在过道中间位置.
  **********************************************************/
 void horizMoveMiddle(float tolerance)
 {
-    float diff = 0.0;
+    float diff = 0.0; //计算左右两边距离差
     float begin_yaw = 0.0;
-    float now_left = 0.0;
+    float now_left  = 0.0;
     float now_right = 0.0;
 
     turnFlag = HORIZ_MOVE;
@@ -296,13 +296,13 @@ void horizMoveMiddle(float tolerance)
     begin_yaw = yawSrv.response.yaw;
 
     distSrvClient.call(distSrv);
-    now_left = distSrv.response.left_dist;
-    ros::Duration(0.02).sleep();//在获取两个传感器值之间需要加延迟
+    now_left = distSrv.response.left_dist; //获取左边距离
+    ros::Duration(0.02).sleep();//在获取两次传感器值之间需要加延迟
     distSrvClient.call(distSrv);
-    now_right = distSrv.response.right_dist;
+    now_right = distSrv.response.right_dist;//获取右边距离
 
     diff = now_left - now_right;
-    ROS_INFO("diff:%f,left:%f,right:%f",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
@@ -320,11 +320,11 @@ void horizMoveMiddle(float tolerance)
 
         distSrvClient.call(distSrv);
         now_left = distSrv.response.left_dist;
-        ros::Duration(0.02).sleep();//在获取两个传感器值之间需要加延迟
+        ros::Duration(0.02).sleep();//在获取两次传感器值之间需要加延迟
         distSrvClient.call(distSrv);
         now_right = distSrv.response.right_dist;
 
-        diff = now_left - now_right;
+        diff = now_left - now_right; //计算左右距离差值
     }
     //当平移结束后立刻停止移动
     publishTwistCmd(0, 0, 0); //stop move
@@ -335,11 +335,12 @@ void horizMoveMiddle(float tolerance)
  *   进行连续两次旋转,在每次转弯后必须有个直行.这里需要注意的是当小车在转弯
  *   后一定要有一个可以修正转弯角度的功能,否则转弯后角度不准确小车前进方向
  *   会错.这里的走迷宫策略是左转优先,中间是直行,最后是右转.
+ *   当前方,左边,右边都无法移动时,进行掉头动作,然后直行.
  **************************************************************************/
 void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         float tolerance, float warn_distance)
 {
-    static int flag = 0;
+    static int flag = 0; //记录是否转过弯的标志
 
     yawSrvClient.call(yawSrv); //通过服务调用来获取yaw值
     start_yaw_data = yawSrv.response.yaw; //记录当前yaw值,用于后面修正
@@ -349,21 +350,21 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
     {
         ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
         turnFlag = TURN_LEFT;
-        flag = 1;
+        flag = 1; //设置转弯标志
     }
-    else if(infrared_f > tolerance*5.0)//判断能否直行
+    else if(infrared_f > warn_distance)//判断能否直行
     {
         //始终控制小车保持在过道的中间位置
         ROS_WARN("go Forward ! yaw_data: %f", start_yaw_data);
         horizMoveMiddle(tolerance);
         turnFlag = GO_FORWARD;
-        flag = 0;
+        flag = 0; //设置直行标志
     }
     else if((infrared_r == (float)0.3)&&(flag != 1))//判断能否右转
     {
         ROS_WARN("turn Right ! yaw_data: %f", start_yaw_data);
         turnFlag = TURN_RIGHT;
-        flag = 1;
+        flag = 1; //设置转弯标志
     }
     else //小车掉头
     {
@@ -401,7 +402,7 @@ int main(int argc, char **argv)
     std::string infrared_topic;
     std::string cmd_topic;
     std::string yaw_service;
-    std::string dist_service;
+    std::string infrared_service;
 
     ros::init(argc, argv, "infrared_maze_node");
     ros::NodeHandle handle;
@@ -409,7 +410,7 @@ int main(int argc, char **argv)
     ros::param::get("~infrared_topic", infrared_topic);
     ros::param::get("~cmd_topic", cmd_topic);
     ros::param::get("~yaw_service", yaw_service);
-    ros::param::get("~dist_service", dist_service);
+    ros::param::get("~infrared_service", infrared_service);
     ros::param::get("~linear_x",  linear_x_speed);
     ros::param::get("~linear_y",  linear_y_speed);
     ros::param::get("~angular_speed", angular_speed);
@@ -417,11 +418,12 @@ int main(int argc, char **argv)
     ros::param::get("~tolerance_dist", tolerance_dist);
     ros::param::get("~odom_frame", odomFrame);
     ros::param::get("~base_frame", baseFrame);
+    ros::param::get("~forward_dist", forward_dist);
 
     ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
     yawSrvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
-    distSrvClient = handle.serviceClient<ros_arduino_msgs::GetInfraredDistance>(dist_service);
+    distSrvClient = handle.serviceClient<ros_arduino_msgs::GetInfraredDistance>(infrared_service);
 
     ros::spin();
     return 0;