更新走迷宫代码

src/infrared_maze/src/infrared_maze.cpp

@@ -94,7 +94,7 @@ void goForward()
     tf::TransformListener tf_Listener;
     tf::StampedTransform tf_Transform;
     float dist = 0.0;
-    float check_dist = 0.5;
+    float check_dist = 0.50;
 
     try
     {
@@ -214,61 +214,39 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         float tolerance)
 {
     static int flag = 0;
-    static int cnt = 0;
+    ros::Duration(0.5).sleep();
 
-    if((infrared_l == (float)0.3)&&(!flag))
+    if((infrared_l == (float)0.3)&&(flag != 1))
     {
-        cnt++;
-        ROS_WARN("Ready turn Left !");
-        if(cnt >= 7)
-        {
-            start_yaw_data = now_yaw_data;
-            ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
-            controlTurn(TURN_LEFT);
-            flag = 1;
-            cnt = 0;
-        }
-        if(infrared_f > tolerance*4.0)
-        {
-            ROS_WARN("continue go forward !");
-            //始终控制小车保持在过道的中间位置
-            horizMoveMiddle(infrared_l, infrared_r, tolerance);
-            publishTwistCmd(linear_x_speed, 0, 0); //go forward
-        }
+        start_yaw_data = now_yaw_data;
+        ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
+        controlTurn(TURN_LEFT);
+        turnFlag = TURN_LEFT;
+        flag = 1;
     }
-    else if(infrared_f > tolerance*4.0)
+    else if(infrared_f > tolerance*5.0)
     {
         //始终控制小车保持在过道的中间位置
         horizMoveMiddle(infrared_l, infrared_r, tolerance);
-        publishTwistCmd(linear_x_speed, 0, 0); //go forward
+        //publishTwistCmd(linear_x_speed, 0, 0); //go forward
+        goForward();
         flag = 0;
         turnFlag = GO_FORWARD;
     }
-    else if((infrared_r == (float)0.3)&&(!flag))
+    else if((infrared_r == (float)0.3)&&(flag != 1))
     {
-        cnt++;
-        ROS_WARN("Ready turn Right !");
-        if(cnt >= 7)
-        {
-            start_yaw_data = now_yaw_data;
-            ROS_WARN("turn Right!, yaw_data:%f", start_yaw_data);
-            controlTurn(TURN_RIGHT);
-            flag = 1;
-            cnt = 0;
-        }
-        if(infrared_f > tolerance*4.0)
-        {
-            ROS_WARN("continue go forward !");
-            //始终控制小车保持在过道的中间位置
-            horizMoveMiddle(infrared_l, infrared_r, tolerance);
-            publishTwistCmd(linear_x_speed, 0, 0); //go forward
-        }
+        start_yaw_data = now_yaw_data;
+        ROS_WARN("turn Right!, yaw_data: %f", start_yaw_data);
+        controlTurn(TURN_RIGHT);
+        turnFlag = TURN_RIGHT;
+        flag = 1;
     }
     else
     {
         start_yaw_data = now_yaw_data;
-        ROS_WARN("turn Back! yaw_data:%f", start_yaw_data);
+        ROS_WARN("turn Back! yaw_data: %f", start_yaw_data);
         controlTurn(TURN_BACK);
+        turnFlag = TURN_BACK;
     }
 }
 
@@ -296,11 +274,11 @@ void yaw_callback(const std_msgs::Float32::ConstPtr& msg)
             sleep_time = fabs(diff)/angular_speed;
             if(diff > 0)
             {
-                angular = -angular_speed;
+                angular = angular_speed;
             }
             else
             {
-                angular = angular_speed;
+                angular = -angular_speed;
             }
         }
         else if(turnFlag == TURN_RIGHT)
@@ -325,6 +303,10 @@ void yaw_callback(const std_msgs::Float32::ConstPtr& msg)
         else //turn back
         {
             target = start_yaw_data + M_PI;
+            if(target > M_PI)
+            {
+                target = target - M_PI*2.0;
+            }
             ROS_INFO("Target Yaw: %f", target);
             diff = target - now_yaw_data;
             sleep_time = fabs(diff)/angular_speed;
@@ -338,7 +320,7 @@ void yaw_callback(const std_msgs::Float32::ConstPtr& msg)
             }
         }
         ROS_WARN("Yaw diff: %f, sleep:%f", diff, sleep_time*2.0);
-        publishTwistCmd(0, 0, angular);
+        publishTwistCmd(0, 0, angular*2);
         ros::Duration(sleep_time*2.0).sleep();
         ROS_WARN("Correct Yaw diff over !");
         publishTwistCmd(0, 0, 0);
@@ -361,6 +343,11 @@ void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
     checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist);
 }
 
+/************************************************************
+ * Description:主函数初始化节点,读取配置参数,订阅和发布话题,
+ *   在订阅的红外测距数据话题回调函数中进行移动,在订阅的yaw
+ *   话题中进行转弯角度的修正.
+ ************************************************************/
 int main(int argc, char **argv)
 {
     std::string infrared_topic;
@@ -384,16 +371,7 @@ int main(int argc, char **argv)
     ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
     ros::Subscriber sub_yaw = handle.subscribe(yaw_topic, 1, yaw_callback);
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-#if 0
-    controlTurn(TURN_RIGHT);
-    ros::Duration(3).sleep(); //300 ms
-    controlTurn(TURN_RIGHT);
-    ros::Duration(3).sleep(); //300 ms
-    controlTurn(TURN_RIGHT);
-    ros::Duration(3).sleep(); //300 ms
-    controlTurn(TURN_RIGHT);
-    ros::Duration(3).sleep(); //300 ms
-#endif
+
     ros::spin();
     return 0;
 }

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/cfg/param.yaml

@@ -1,4 +1,4 @@
-###############################################
+##################################################
 # Copyright: 2016-2020 www.corvin.cn ROS小课堂
 # Description:使用IIC总线来读取IMU模块的数据,并
 #   通过话题将imu数据发送出来.这里是可以配置的
@@ -6,9 +6,10 @@
 # Author: corvin
 # History:
 #   20200406:init this file.
-################################################
+#   20200410:更新默认发布imu话题数据频率为20hz.
+###################################################
 pub_data_topic: imu_data
 pub_temp_topic: imu_temp
 yaw_topic: yaw_data
 link_name: base_imu_link
-pub_hz: 10
+pub_hz: 20

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

@@ -12,7 +12,6 @@
 #    20191031: Initial this file.
 #    20191209: 新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
 #    20200406: 增加可以直接发布yaw数据的话题.
-
 import rospy
 import math
 
@@ -31,7 +30,7 @@ imu_yaw_calibration = 0.0
 def reconfig_callback(config, level):
     global imu_yaw_calibration
     imu_yaw_calibration = config['yaw_calibration']
-    rospy.loginfo("Set imu_yaw_calibration to %d" % (imu_yaw_calibration))
+    rospy.loginfo("Set imu_yaw_calibration to %f" % (imu_yaw_calibration))
     return config
 
 rospy.init_node("imu_node")
@@ -40,7 +39,7 @@ rospy.init_node("imu_node")
 data_topic_name = rospy.get_param('~pub_data_topic', 'imu_data')
 temp_topic_name = rospy.get_param('~pub_temp_topic', 'imu_temp')
 link_name  = rospy.get_param('~link_name', 'base_imu_link')
-pub_hz = rospy.get_param('~pub_hz', '10')
+pub_hz = rospy.get_param('~pub_hz', '20')
 yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
 
 data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)