修复动态控制行进距离不能停止的bug

src/robot_dynamic/src/robot_dynamic.cpp

@@ -1,7 +1,7 @@
 /*
  * @Author: adam_zhuo
  * @Date: 2020-08-31 09:40:13
- * @LastEditTime: 2020-08-31 19:09:26
+ * @LastEditTime: 2020-09-04 09:36:15
  * @LastEditors: Please set LastEditors
  * @Description: 动态调节测试小车转向移动是否准确
  * @FilePath: /blackTornadoRobot/src/robot_dynamic/src/robot_dynamic.cpp
@@ -136,7 +136,8 @@ void robot_move(int move_flag,const float check_dist)
     float x_start = tf_Transform.getOrigin().x();
     float y_start = tf_Transform.getOrigin().y();
 
-    while(fabs((dist-fabs(check_dist)))>tolerance_dist&&(ros::ok()))
+    bool dist_flag = true;
+    while(dist_flag&&(ros::ok()))
     {
         switch(move_flag){
             case VERTICAL_MOVE:
@@ -154,7 +155,9 @@ void robot_move(int move_flag,const float check_dist)
         float y = tf_Transform.getOrigin().y();
         dist = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
         ROS_INFO("Go forward, dist:%f", dist);
-
+        if(fabs(check_dist)-dist<tolerance_dist){
+            dist_flag = false;
+        }
     }
     publishTwistCmd(0, 0, 0); //stop move
     ROS_INFO("Go forward finish !!!");